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ABSTRACT 


An  autopilot  is  designed  using  Sliding  Mode  Control  Theory  that  can  control  the 
speed  and  heading  of  a  full  six  degree  of  freedom,  nonlinear  model  of  the  Navy’s  SDV 
Marie  9  vehicle.  The  control  laws  are  based  on  a  simplified  linear  model  that  allows  speed 
and  heading  to  be  treated  as  separate  systems.  Once  control  of  heading  and  speed  is 
established,  depth  control  is  then  achieved  by  a  third  control  law.  Although  they  are 
developed  separately,  the  three  individual  control  laws  act  simultaneously  to  provide 
robust  control  of  speed,  heading,  and  depth  of  the  nonlinear  model  of  the  vehicle.  Line 
of  Sight  Guidance  is  used  to  convert  the  way  points  provided  by  the  mission  planner  into 
commands  for  heading  to  which  the  autopilot  responds.  The  performance  of  the  autopilot 
is  evaluated  over  a  wide  range  of  speeds  to  demonstrate  its  robustness.  In  addition,  the 
effects  of  current  are  simulated  and  the  autopilot  is  modified  to  compensate  for  the 
presence  of  a  strong  current. 
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I.  INTRODUCTION 


A.  GENERAL 

There  has  been  an  increased  interest  recently  in  the  need  for  autonomous  underwater 
vehicles  (AUV'i  in  both  the  Navy  and  private  industry.  A  variety  of  unclassified  missions 
include  ASW,  decoy,  survey,  reconnaissance,  and  ocean  engineering  work.  As  the  cost 
of  manned  submarines  increases,  there  are  significant  advantages  to  using  cheaper 
unmanned  vehicles.  The  AUV  should  be  able  to  maneuver  freely  in  the  ocean 
environment  with  respect  to  speed,  heading,  and  depth  in  order  to  carry  out  its  missions. 
Such  maneuvering  requirements  have  to  be  easily  accomplished  by  a  low  level  active 
control  system,  and  in  the  presence  of  environmental  and  physical  uncertainty. 

All  information  concerning  the  environment  of  a  vehicle  is  detected  by  the  sensing 
instrumentation  on  board  the  vehicle  and  sent  to  the  higher  level  intelligence  systems  to 
be  interpreted  in  order  to  carry  out  the  mission.  The  dynamics  of  underwater  vehicles  are 
described  by  highly  nonlinear  systems  of  equations  with  uncertain  coefficients  and 
disturbances  that  are  difficult  to  measure.  Robust  control  using  Sliding  Mode  Control 
Theory  is  reputed  to  provide  accurate  control  of  nonlinear  systems  despite  unmodeled 
system  dynamics  and  disturbances  making  it  a  likely  candidate  for  designing  the  control 
laws  that  will  govern  the  autopilot  function  of  unmanned  vehicles. 
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B.  AIM  OF  TfflS  STUDY 


The  aim  of  this  thesis  is  to  develop  a  robust  autopilot  that  will  control  speed, 
heading,  and  depth  by  the  application  of  Sliding  Mode  Control  Theory.  The  full  six 
degree  of  freedom  equations  of  motion  that  describe  the  vehicle  are  simplified  and 
linearized  to  the  point  that  they  can  be  treated  as  three  separate,  independent,  single  input 
single  output  systems.  The  control  laws  for  speed,  heading,  and  depth  are  developed 
independently  and  then  combined  together  to  attain  effective  control  of  a  simple  linear 
model  first  and  ultimately  the  complete  nonlinear  model  for  the  vehicle. 

Once  the  autopilot  is  verified,  it  is  coupled  with  a  Line  of  Sight  Guidance  scheme 
to  allow  the  autopilot  to  interface  with  the  higher  levels  of  decision  making  that  provide 
way  points  to  describe  a  desired  path  for  the  vehicle.  The  merits  of  the  total  system  are 
then  evaluated  for  robustness  in  terms  of  the  range  of  operating  speed  over  which  it  can 
successfully  control  the  vehicle  when  compared  to  the  nominal  operating  speed  about 
which  the  equations  of  motion  were  initially  linearized. 

Finally,  the  effect  of  current  on  Line  of  Sight  Guidance  is  explored  and  refinements 
made  resulting  in  the  development  of  a  highly  robust  and  effective  system  for  controlling 
the  next  generation  of  autonomous  underwater  vehicles  under  construction  at  the  Naval 
Postgraduate  School  and  elsewhere  in  private  industry. 

C.  THESIS  OUTLINE 

In  Chapter  2  the  equations  of  motion  for  the  full  six  degree  of  freedom  model  of 
the  AUV  are  simplified  and  linearized.  A  model  for  the  vehicle  is  then  built  using  the 
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Matrix-x  software  package  (Copyright  1989  by  Integrated  Systems  Inc.)  and  sliding  mode 
control  laws  are  developed,  assuming  full  state  feedback,  to  control  the  speed  and  heading 
of  this  simple  linear  model. 

Chapter  3  shows  the  application  of  the  control  laws  developed  in  Chapter  2  to  the 
full,  nonlinear,  six  degree  of  freedom  model  that  represents  the  SDV-9  vehicle  currendy 
in  use  by  the  Navy.  In  addition,  the  depth  control  law  which  was  developed  previously 
at  NPS  by  Joo-No  Sur  [Ref.  1]  was  included  to  attain  control  of  depth. 

In  Chapter  4  an  observer  for  sway  velocity  is  designed  and  tested.  It  is  ultimately 
determined  that  sway  velocity  is  better  treated  as  a  disturbance  and  consequently  it  is  set 
to  zero  wherever  it  appears  in  the  control  law  equations. 

Chapter  5  explores  using  Line  of  Sight  Guidance  to  allow  the  way  points  provided 
by  the  planning  function  of  the  AUV  to  be  interpreted  into  commands  for  heading. 

Chapter  6  investigates  the  speed  envelope  of  3  ft /sec  to  50  ft/sec  over  which  the 
control  laws  are  valid  even  though  the  initial  linearization  was  done  around  an  operating 
speed  of  6  ft/sec.  This  shows  the  robustness  of  the  control  laws. 

Chapter  7  shows  the  effect  of  current  on  Line  of  Sight  Guidance  and  develops  a 
modified  control  law,  including  current  compensation,  that  improves  the  autopilot’s 
effectiveness  in  currents  with  speeds  up  to  50%  of  the  operating  speed. 
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H.  LINEAR  MODELING  WITH  MATRIX-X 


A.  INTRODUCTION 

Because  the  vehicle  dynamic  equations  of  motion  are  relatively  easy  to  linearize, 
the  first  step  of  control  design  for  the  autopilot  was  to  consider  the  AUV  to  be 
represented  by  a  nominal  linear  model.  In  linearizing  the  equations  of  motion  it  was  found 
that  the  equations  became  uncoupled  to  the  extent  that  speed  control  and  heading  control 
could  be  addressed  separately.  This  led  to  considering  the  control  of  heading  and  speed 
as  two  independent  SISO  systems.  Using  this  design  approach,  the  two  inputs  provided 
to  the  autopilot  are  the  desired  heading  and  the  desired  speed.  No  attempt  is  made  to 
control  depth  at  this  stage  in  the  autopilot  design.  In  fact,  it  is  assumed  that  all  motion 
is  confined  to  the  horizontal  plane.  The  block  diagram  representing  the  autopilot  is 
shown  in  Figure  1.  This  is  the  actual  Matrix-x  system  build  diagram  in  superblock  format. 
The  remaining  portion  of  this  chapter  will  be  devoted  to  explaining  each  of  these  blocks 
individually  as  the  autopilot  control  design,  based  on  the  nominal  model,  is  described. 

B.  AUV  DYNAMIC  MODEL 

1.  Identification  of  symbols 

All  the  symbols  that  will  be  used  to  develop  the  linear  model  for  the  AUV  are 
identified  in  the  execution  program  shown  in  Appendix  A.  They  are  listed  along  with 
their  units  and  in  the  case  of  constant  parameters,  numerical  values  are  given. 
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Figure  1.  Autopilot  Block  Diagram 
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2.  Simplification  and  linearization 

The  nonlinear  equations  of  motion  for  this  vehicle  were  taken  from  work  done 
previously  at  NPS  by  Boncal  [Ref.  2]  who  used  the  dynamic  model  as  established  by 
Crane,  Summey,  et  al  [Ref.  3],  as  representative  of  the  SDV  Mark  9  vehicle.  While  this 
vehicle  is  not  identical  to  any  specifically  planned  AUV,  it  remains  a  useful  vehicle  for 
the  study  of  dynamics  and  control  issues. 

Only  the  equations  for  motion  in  the  horizontal  plane  are  developed  in  this 
chapter.  In  addition,  the  complete  form  of  these  equations  will  not  be  shown.  Instead,  the 
assumptions  used  to  simplify  them  will  be  stated  and  the  resulting  simplified  equations 
will  be  shown  in  their  linearized  form.  It  should  be  noted  that  the  linearization  was  done 
around  a  constant  speed  u  =  6.0  ft/sec  and  a  corresponding  constant  motor  rpm  n  =  500 
rpm,  with  the  vehicle  in  level  flight. 

a.  Assumptions 

•  Motion  is  confined  to  the  horizontal  plane: 

(w,  p,  q,  0,  <}>,  p,  q,  6b  ,  =  0) 

•  Some  dynamic  coefficients  are  small  and  can  be  neglected: 

(Yp  Jp  ,Y„  Jqr  =  0) 

•  The  AUV  is  neutrally  buoyant. 
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By  design  Xt,Yt,Zt  are  negligible. 


•  Cross  flow  drag  terms  tend  to  cancel  each  other  as  far  as  steering  moments  are 
concerned. 

b.  Longitudinal  equation  of  motion 


ti  =  -a  uu  +  $n6n 


(2.1) 


Where: 


a  =  (  2  p  L2  CM  )  /  (  2  mass  -  p  L3  XA  ) 


=  .00385  +  1.296£-17  [u  LJkvis  -  1.2E-7  ]2 


p  =  (.012)2  a 


c.  Lateral  equation  of  motion 

m,  v  -  m2  f  -  «  v  +  Y2  u  r  +  Y3  6r  (2 -2) 


Where: 


my  =  (  mass  -  p  L3  Y^)  /  2 

ffij  =  (  p  L4  1)  )  /  2 


y,  =  (  P  l2  rv  u  )  i  2 

Y2  =  (  p  L3  Yru)  I  2 
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Y,  =  (  p  L2  r,  P  )  /  2 


d.  Yaw  equation  of  motion 

m3v  +  m/  =  Nx  v  +  N2  r  +N}  6r 


Where: 


m3  =  (-p  LA  N,  )  I  2 


m4  =  /z  -  (  p  L5  N,  )  /  2 


]V,=(pLX«')/2 
N2  =  (  p  L*NrZ)  I  2 
N3  =  (p  IS  \  u2  )  I  2 

e.  Euler  angle  rate 

ijr  =  r 


(2.3) 


(2.4) 


/.  Inertial  position  rates 

x  -  u  cos(\|r)  -  v  sm(ty) 

y  =  «  sin(\|/)  +  v  cos(ty) 


(2.5) 

(2.6) 
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3.  State  space  configuration 


a.  SISOl 

The  state  space  equation  for  system  1  that  will  be  used  to  attain  speed 
control  is  simply  the  longitudinal  equation  of  motion  previously  developed  and  considered 
to  be  in  the  standard  state  space  form  shown  below. 

X,  =  [4,]  X,  +  [B,  1  Ux 


The  components  of  these  matrices  are  given  by  equation  (2.7). 

u  -  [-a  u  ]  u  +  [P  n  ]  6n  (2-7) 


b.  SIS02 

To  find  the  state  space  equation  for  heading  control,  equations  (2.2)  and 
(2.3)  must  be  treated  as  coupled.  This  was  done  as  follows: 


M  = 


m1  m2 
m3  m4 


Yr  Y2  T, 
N,  N2  N3 


Represent  equations  (2.2)  and  (2.3)  as: 
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Consequently,  this  equation  can  be  solved  as  follows: 


(A 

r  V  ' 

1  v  ' 

I  =  [  m  r1  [  c  ] 

r 

•[D] 

r 

VJ 

,  tr  ; 

(2.8) 


Combining  equation  (2.4)  with  equation  (2.8)  results  in  the  state  space  form 
to  be  used  for  heading  control.  Since  two  systems  are  being  dealt  with  at  the  same  time 
this  system  will  be  represented  by  the  following  state  space  equation. 


X2  =  \A2]  X2  *  [B2]  u2 


Where  these  matrices  are  defined  by  equation  (2.9). 


'\jr' 

V 

oo  r 

0  dn  dl2 

V) 

V 

+ 

0 

4, 

/> 

P  ^21  ^22 

Ji 

^Z3 

(2.9) 


Equations  (2.7)  and  (2.9)  are  represented  graphically  in  the  Matrix-x  system 
build  diagram  shown  in  Figure  2.  Again  notice  that  they  represent  two  independent 


systems  which  leads  to  the  next  step  of  determining  the  control  laws  for  heading  control 
and  speed  control  by  considering  them  as  separate  SISO  systems.  The  triangles  represent 
gain  values  calculated  in  the  execution  program  shown  in  Appendix  A. 

C.  SISOl  SLIDING  MODE  SPEED  CONTROL 

At  this  point  the  control  of  speed  is  easily  achieved  using  sliding  mode  control 
theory.  The  basis  of  this  theory  and  its  application  to  controlling  the  dive  maneuvers  of 
AUV’s  is  documented  by  Joo-No  Sur  [Ref.  1].  Consequently,  the  theory  of  sliding  mode 
control  will  not  be  included  here  but  rather  its  application  will  be  shown.  Further  details 
are  given  in  the  tutorial  by  Decarlo  et  al.  [Ref.  4],  and  in  the  paper  by  Yoeger  and  Slotine 
[Ref.  5]  for  application  to  underwater  vehicles. 

Recalling  from  equation  (2.7)  that  the  state  vector  for  this  system  is  simply  the 
surge  velocity  (u)  and  the  input  is  the  motor  tpm  (8J,  the  equation  for  the  sliding  surface 
is  written  as  a  weighted  sum  of  the  error  between  the  state  variables  and  their  desired 
values. 

Oj  =  $,(«  -  Wj)  (2.10) 

According  to  Liapunov  Stability  [Ref.  6],  global  asymptotic  stability  of  the  speed  error 
is  guaranteed  by  the  condition: 

o,(0  o,(0  <  0 
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For  SISOl  this  results  in  the  following  equation. 


®i(0  =  "(n?)  sgn(o j(0) 


(2.11) 


The  symbol  tj,  represents  the  sliding  coefficient  for  the  speed  control  law.  To  eliminate 
chattering,  the  sgn  function  can  be  replaced  without  loss  of  generality  by  the  Matrix-x 
function  satsgn.  Satsgn  still  acts  like  a  switch,  but  has  the  advantage  of  allowing  a  linear 
increase  to  the  prescribed  saturation  value  which  in  this  case  is  unity.  As  a  result  of  this 
substitution,  and  considering  the  time  dependence  of  a,  to  be  understood, the  expression 
now  has  the  form: 


6,  =  -(tij)  satsgn(o 


(2.12) 


The  speed  control  law  can  be  obtained  by  setting  the  derivative  of  Equation  (2.10)  equal 
to  Equation  (2.12)  and  substituting  for  u  from  the  state  space  equation  for  SISOl.  The 
resulting  expression  for  the  motor  input  is: 


=  [  -TiJ  satsgnioj  -slauu]/(?l$n) 


(2.13) 


At  this  point  the  values  of  tj,  and  s,  must  be  determined.  Since  there  is  only  one 
state  variable  to  consider,  s,  =  1.  The  choice  of  T|,  will  be  made  through  trial  and  error 
using  Matrix-x  simulation.  In  general,  increasing  T|,  will  make  the  system  more  responsive 
while  reducing  it  will  make  the  system  more  sluggish. 
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The  speed  control  law  is  represented  in  Matrix-x  system  build  in  Figure  3.  Notice 
that  the  control  signal  (8.)  is  made  up  of  two  parts,  one  representing  the  linear  feedback 


term  and  the  other  the  nonlinear  switching  term  as  shown  as  follows: 


8 


n 


+  8 


n 


(2.14) 


Notice  too,  that  an  upper  limit  of  500  rpm  has  been  included  in  the  system  build  to 
simulate  the  maximum  allowed  motor  rpm. 


Figure  3.  SISOl  Sliding  Mode  Speed  Control  Block  Diagram 


D.  SIS02  SLIDING  MODE  HEADING  CONTROL 

Sliding  mode  control  theory  is  used  to  achieve  heading  control  by  using  the  state 
space  equation  for  SIS02.  First  the  equation  for  the  sliding  surface  is  written  as  a 
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weighted  sum  of  the  error  between  the  state  variables  and  their  desired  values  where  the 
weights  are  the  coefficients  of  the  sliding  surface  [Ref.  4]. 

o2  =  52(t|r  -  )  +  j3(v  -  vd  )  +  s4(r  -  rd  )  (2.15) 

This  equation  is  simplified  by  noting  that  the  desired  values  of  sway  velocity  and  angular 
velocity  are  zero.  By  making  this  substitution  the  sliding  surface  equation  for  heading 
control  becomes: 

o2  =  s2(\ |t  -  )  +  s3(v)  +  s4(r)  (2.16) 

Again  using  Liapunov  Stability  Theory  to  meet  the  requirements  for  global  asymptotic 
stability  the  following  expression  is  obtained  for  SIS02. 

02  «  ~(n2)2  sotsgn(a2)  (2-17) 

Next,  b2  is  found  by  taking  the  derivative  of  equation  (2.16),  realizing  that  yd,  at  this 
stage,  is  considered  to  be  a  constant. 

d2  =  s2(t|f)  +  53(v)  +  s4(r)  (2.18) 

By  equating  equation  (2.17)  and  (2.18)  and  substituting  from  the  SIS02  state  space 
equation  for  the  derivatives  of  the  state  variables  the  heading  control  law  is  obtained  as 
shown. 
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5 


[-(T|2)2  satsgn(o2)  -  (s2  +  s3du  +  s4d2l)r  ] 
(s3da  +  s4d23) 


(2.19) 


Notice  again,  that  this  control  law  can  again  be  considered  to  have  two  distinct  parts  as 
shown  below. 

8=6+6 
r  r  r 

The  linear  feedback  part  of  the  control  law  is  designed  to  give  the  system  the 
desired  dynamics  while  on  the  sliding  surface.  The  nonlinear  term  is  responsible  for 
getting  the  system  to  the  sliding  surface  initially  and  compensating  for  any  unmodeled 
behavior  that  would  drive  the  vehicle  off  of  the  sliding  surface  once  it  got  there.  Once  on 
the  sliding  surface,  there  is  no  need  for  the  nonlinear  term  unless  there  is  uncertainty  in 
the  model  [Ref.  4].  The  equations  for  both  of  these  terms  are  as  follows: 

6r  =  ~[NNX  r  +  NN2  v]  IDD2  <2J0) 

6r  =  (-(ti 2)2IDD2)  satsgn(o2)  <221) 

Where: 

NNX  =  s2  +  j3  dn  +  s4  djj 
NN2  =  s3  dn  +  s4  d22 
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D&2  ~  S3  ^13  +  54  4>3 

Unlike  the  speed  control  subsystem,  the  sliding  surface  for  heading  control  is 
multidimensional.  In  this  case  the  value  for  ST=[s2,S3,s4]T  is  determined  by  considering 
the  closed  loop  dynamics  of  this  system  in  the  sliding  condition  (c,o  =  0).  Under  this 
condition  the  state  space  equation  for  SIS02  becomes: 


X,  -  U2]  Xi  ♦  [fi2] 


(222) 


By  writing  equation  (2.20)  in  terms  of  ST  and  the  state  space  matrices,  the  following 
relationship  is  attained. 


K  =  ~(STB2y'  sta2  x2 


(223) 


Combining  equations  (2.22)  and  (2.23)  gives: 


X,  =  [A2  -  BJS  T  B,)'1  StA2  1  X. 


(224) 


Note  that  equation  (2.24)  is  in  the  form: 


X2  =  [A2  -  b2k\x2 


Where: 


K  =  (S'  B2y'  S7  A2 


Consequently,  equation  (2.24)  can  be  represented  as: 
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x2  =  [AC2]  x2 


In  this  form  [AC2]  is  the  closed  loop  dynamics  matrix.  At  this  point  K  can  be  found 
using  pole  placement  techniques.  This  is  done  by  specifying  the  desired  poles  that  will 
determine  the  behavior  on  the  sliding  plane.  It  should  be  noted  here  that  in  accordance 
with  Sliding  Mode  Theory,  one  of  these  poles  must  be  at  the  origin  thus  leaving  2  poles 
to  determine  the  closed  loop  bandwidth  and  damping  [Ref.  4].  Using  Matrix -x  this  is  done 
as  follows: 

K  =  POLEPLACE(AC2J2J>OLES2) 

Now  that  a  value  for  K  can  be  found,  ST  is  obtained  by  multiplying  both  sides  of 
the  equation  for  K  by  STB2  .  This  gives  the  result: 

S  T  [AC2]  =  0 

Which  is  also  correctly  expressed  as  equation  (2.25). 

[  AC2  ]tS  =  0  (2-25) 

At  this  point  the  fact  that  [AC2]T  has  one  eigenvalue  that  is  zero  is  utilized  to 
complete  the  solution  of  S  by  considering  the  standard  eigenvalue  problem  ([AC.)1  -I)V 
=  0.  For  the  case  when  the  eigenvalue  is  zero,  [AC2]TV  =  0  .  Comparing  this  expression 
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with  equation  (2.25)  it  becomes  apparent  that  the  eigenvector  V  in  the  general  case 
becomes  the  vector  S  for  our  circumstances  [Ref.  7].  Consequently,  S  is  determined  by 
the  eigenvalue  function  available  in  Matrix-x.  This  calculation  is  performed  within  the 
execution  program  shown  in  Appendix  A. 

The  next  step  is  to  test  the  control  law  via  simulation  by  adjusting  the  values  of  r|2 
and  the  poles  to  obtain  the  desired  control  characteristics.  The  Matrix-x  system  build 
representation  of  the  heading  controller  that  was  used  for  simulation  is  shown  in  Figure 
4.  Notice  that  the  satsgn  function  has  been  modified.  Its  slope  has  been  adjusted  to 
provide  a  filter  for  numerical  chattering.  The  saturation  limit  of  the  satsgn  function 
represents  the  boundary  layer  thickness  4>  as  developed  by  Slotine  [Ref.  5].  Since  the 
range  of  values  of  the  satsgn  function  is  between  -1  and  +l,a  multiplication  factor  of  1/<|> 
must  also  be  included. 

Selection  of  the  boundary  layer  thickness  is  a  design  issue.  In  Figure  4,  the  value 
is  set  at  .1  based  on  many  simulation  runs.  This  in  turn  requires  a  multiplication  factor 
of  10  to  be  included  in  the  block  diagram. 


E.  SIMULATION  RESULTS 

The  goal  for  the  controller  was  to  reach  the  desired  heading  and  speed  in  the 
shortest  time  possible  and  avoid  inefficient  use  of  the  control  inputs  while  doing  so.  Many 
simulation  runs  were  conducted  to  obtain  the  best  values  for  rj,.  rj2.  and  the  poles  for 
the  dynamics  of  the  sliding  surface  of  SIS02.  In  addition,  reasonable  values  for  the 
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boundary  layer  thicknesses  and  for  the  satsgn  functions  had  to  be  determined. 

The  output  from  the  simulation  is  displayed  in  a  set  of  six  graphs.  The  graph  of 
Sliding  Coefficient  vs  Time  gives  insight  into  the  role  that  the  T|  and  the  choice  of  poles 
play  in  minimizing  the  amount  of  time  it  takes  to  reach  the  desired  heading.  The  value 
of  T|  primarily  affects  the  time  it  takes  to  reach  the  sliding  plane  where  c(x)  =  0.  The 
poles  that  are  selected  also  effect  the  time  it  takes  to  reach  the  sliding  plane  by  effecting 
its  slope  but  their  primary  influence  is  on  the  dynamics  of  the  system  as  it  travels  along 
the  sliding  plane  to  reach  the  desired  speed  and  heading.  This  shows  the  need  to  balance 
the  value  of  r|  and  the  poles  to  obtain  the  optimal  behavior  for  the  control  system.  It 
appears  as  though  a  threshold  for  T)  exists  above  which  the  time  to  reach  the  sliding  plane 
no  longer  decreases  and  the  only  advantage  that  is  gained  by  increasing  T|  would  be  to 
increase  the  robustness  of  the  controller  as  it  tries  to  maintain  the  system  on  the  sliding 
plane  in  the  presence  of  unmodeled  dynamics  and  disturbances.  Recalling  that  no 
uncertainties  have  been  built  into  this  model  yet,  the  choice  of  the  best  tj  values  is  based 
on  response  time  alone.  Initial  runs  caused  a  value  of  .6  to  be  selected  for  rj,1  and  .4  for 
t|22.  Using  these  values  additional  simulations  revealed  the  best  choice  of  poles  for  SIS02. 
Heading  control  was  attained  with  a  pole  selection  of  [0,.41,.42].  A  more  detailed 
description  of  the  effects  of  each  of  these  elements  of  a  sliding  mode  controller  along 
with  simulation  runs  to  reveal  their  effects  appears  in  [Ref.  1]. 

This  control  law  was  used  for  the  simulation  run  shown  in  Figure  5.  The  Airv  was 
initially  at  rest  and  commanded  to  attain  a  constant  speed  of  4  ft/sec.  While  the  speed 
controller  was  accomplishing  that  task  the  heading  controller  was  independently  changing 
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the  heading  to  90  degrees  from  an  initial  value  of  zero.  The  results  show  a  good  response 
with  efficient  use  of  the  control  inputs.  These  results  are  typical  of  the  many  trials  that 
were  run  to  verify  the  control  laws.  The  next  step  is  to  apply  these  control  laws  based  on 
a  simplified,  linear  model,  to  a  more  realistic  nonlinear  model  to  see  if  they  can  still  be 
effective  in  controlling  speed  and  heading. 
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Figure  5.  Simulation  Results  for  Linear  AUV 


m.  CONTROLLING  THE  NONLINEAR  AUV 


A.  INTRODUCTION 

Two  main  questions  are  addressed  in  this  chapter.  First,  will  the  sliding  mode 
control  laws  for  speed  and  heading  developed  in  Chapter  2  using  a  simplified  linear 
model  for  the  AUV  be  able  to  control  a  more  realistic  nonlinear  model  of  the  AUV.  The 
nonlinear  model  that  is  used  was  originally  developed  by  Boncal  [Ref.  2],  based  on  the 
Mk  9  swimmer  delivery  vehicle  and  most  recently  used  by  Joo-No  Sur  [Ref.  1]  in  his 
design  of  a  SMC  depth  controller.  It  is  in  the  form  of  a  Fortran  code  and  is  shown  in 
Appendix  B. 

The  second  question  to  be  answered  is  whether  the  control  laws  for  speed  and 
heading  control  can  be  simply  inserted  into  the  Fortran  model  as  two  independent 
modules  and  work  harmoniously  with  the  depth  controller  already  present.  If  this  is 
possible  then  the  task  of  three  dimensional  control  of  the  nonlinear  six  degree  of  freedom 
model  can  be  accomplished  by  3  simple  SISO  systems  rather  than  a  complicated  MIMO 
system.  Recalling  that  many  simplifying  assumptions  were  made  to  develop  the  linear 
model  that  are  no  longer  valid  for  the  nonlinear  model  and  that  some  of  the  assumptions 
for  one  of  the  SISO  systems  may  be  in  direct  conflict  with  the  others,  it  is  likely  that 
significant  unmodeled  behavior  will  have  to  be  handled  by  the  nonlinear  switching  terms 
in  the  SMC  control  laws. 
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B.  SPEED  CONTROL  [SISOl] 


Recall  from  Chapter  2  that  the  best  value  of  t|t*  was  .6.  Utilizing  Matrix-x  to  get 


the  necessary  coefficients,  the  speed  control  law  was  obtained  and  programmed  into  the 


Fortran  code  as  shown  in  Figure  6.  Notice  the  use  of  IF  THEN  statements  to  provide  the 


satsgn  function  and  to  establish  an  RPM  limit. 


*******<,RPM  INPUT  calculation  ********* 
SS1-U-UD 

I F ( ABS ( SSI )  .LT.  1.0)  SATSGNl- ( SSI/1 ) 
IF( SSI  .LE.  -1.0)  SATSGNl  — 1.0 
I F( SSI  .GE.  1.0)  SATSGN1-1.0 
RPM— 1153.9*SATSGN1  +  83.33*U 
IF  (RPM  .GE.  500.0)  RPM=  500.0 
IF  (RPM  .LE.  -500.0)  RPM— 500 . 0 


Figure  6.  Sliding  Mode  Speed  Control  Fortran  Code 


C.  HEADING  CONTROL  (SIS02J 

The  Matrix-x  simulations  provided  the  necessary  values  for  die  control  law.  Recall 
that  a  value  of  .4  was  chosen  for  Tjj1  and  that  the  pole  location  of  [0,  -.41, -.42]  was 
selected.  This  resulted  in  the  following  heading  control  law  shown  in  Figure  7  that  was 
programmed  into  the  Fortran  model.  Notice  that  the  maximum  rudder  deflection  lias  been 
limited  to  ±  .4  radians. 


25 


*  *  *  *  *  *  * RUDDER  INPUT  CALCULATION*  *  *  *  *  *  * 
SS2«.5732*( PS I -HD )  -  .0739*V  +  .8161*R 
I F ( ABS ( SS2 )  .LT.  .1)  SATSGN2-( SS2/. 10 ) 
IF( SS2  .LE.  -.1)  SATSGN2— 1.0 
IF( SS2  .GE.  .1)  SATSGN2-1.0 


DR-2.5785*SATSGN2  +  .0328*V 

IF  (DR  .GE.  0.4)  DR  -  0.4 

IF  (DR  .LE.  -  0.4)  DR  -  -0.4 


. 1112*R 


Figure  7.  Sliding  Mode  Heading  Control  Fortran  Code 


D.  SIMULATION 

With  these  two  primary  modifications  to  the  Fortran  model,  the  same  maneuver  that 
was  shown  in  Chapter  2  using  the  linear  model  was  now  attempted  for  the  nonlinear 
AUV.  The  control  laws  were  very  successful  at  achieving  the  desired  speed  and  heading 
as  can  be  seen  by  Figure  8.  The  effects  of  the  unmodeled  terms  were  present,  but 
stabilized  by  the  nonlinear  switching  term  in  the  control  laws  confirming  the  robust  nature 
of  sliding  mode  control  that  make  it  particularly  desirable  for  this  application.  The 
response  was  more  sluggish  and  more  control  effort  was  required  than  for  the  linear 
model.  This  was  expected  however  since  the  nonlinear  model  contains  terms  to  represent 
the  increased  loss  in  a  turn  which  the  linear  model  did  not.  A  comparison  to  Figure  5,  in 
Chapter  2,  shows  that  it  took  about  15  seconds  longer  to  reach  the  desired  speed  and 
almost  30  seconds  longer  to  reach  the  desired  heading.  These  results  seem  very 
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Figure  8.  Simulation  Results  for  Nonlinear  AUV 


satisfactory  since  the  use  of  the  motor  and  rudder  inputs  were  at  their  maximum  values 
throughout  most  of  the  run  and  consequently  utilized  to  their  full  capacity  to  reach  the 
desired  conditions. 

To  answer  the  second  question  the  same  maneuver  was  commanded  with  a  depth 
change  of  20  feet  included.  It  should  again  be  pointed  out  that  the  depth  control  law  was 
developed  independently  by  Joo-No  Sur  and  used  without  any  modification  [Ref.  1].  The 
results  are  shown  in  Figures  9  and  10.  Notice  that  the  depth  control  law  accomplished 
the  commanded  depth  change  independent  of  the  course  and  speed  adjustments.  The 
control  effort  of  the  dive  plane-  seems  very  efficient  as  does  the  use  of  the  rudder  and 
motor  RPM.  All  three  of  aese  control  laws  are  acting  simultaneously  and  independently 
but  together  they  produce  the  desired  result. 

To  fiuther  demonstrate  the  independence  of  these  3  SISO  systems,  two  additional 
runs  are  shown.  First,  the  20  foot  depth  change  is  commanded  without  requiring  a  change 
in  heading.  The  depth  change  characteristics  are  then  compared  to  those  for  the  previous 
simulation  in  Figure  11.  The  left  column  is  the  run  with  a  depth  change  only  and  the 
right  column  shows  the  depth  change  while  the  vehicle  is  also  making  a  90  degree 
heading  correction.  Notice  that  the  results  are  almost  identical  showing  that  depth  control 
is  not  adversely  affected  by  a  change  in  heading. 

Similarly,  a  90  degree  turn  was  commanded  without  requiring  a  depth  change  and 
compared  to  the  combined  diving  and  turning  maneuver.  The  results  are  compared  in 
Figure  12.  The  left  column  represents  the  turn  only  while  the  right  column  shows  the 
same  turn  during  a  20  foot  depth  change.  Again  the  results  are  nearly  identical  showing 
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Figure  9.  Simulation  Including  Depth  Change  (Nonlinear  AUV) 


o 


0.0  JO. 0  20.0  30.0  40.1)  50.0 


TIME  (SEEl 


TIME  f SEC l 


O 


TIME  I  SEE l 


Figure  12.  Effect  of  Depth  Changes 
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that  heading  control  is  not  adversely  affected  by  a  simultaneous  change  in  depth. 

E.  CONCLUSION 

In  view  of  these  results  it  can  be  concluded  that  the  desired  three  dimensional 
control  of  the  full  six  degree  of  freedom  nonlinear  vehicle  can  be  accomplished  by  3 
SISO  systems  thus  avoiding  the  need  to  model  the  AUV  as  a  MIMO  system. 

Some  problems  still  remain  however  that  will  be  addressed  in  subsequent  chapters. 
First,  the  control  of  heading  is  based  on  assuming  full  state  feedback  which  includes  sway 
velocity.  To  be  realistic,  since  sway  velocity  is  not  directly  measurable,  an  observer  will 
have  to  be  designed.  In  addition,  a  more  global  objective  is  to  ultimately  cause  the  AUV 
to  travel  to  a  desired  way  point  rather  than  simply  achieving  a  desired  heading.  Though 
our  approach  of  developing  the  control  laws  as  separate  SISO  systems  has  been  validated 
at  this  point,  it  still  remains  to  develop  a  guidance  scheme  that  can  interface  with  these 
control  laws  to  achieve  path  following  in  the  horizontal  plane. 

The  design  of  the  sway  velocity  observer  is  dealt  with  in  Chapter  4  and  the  issue 
of  guidance  in  Chapter  5. 
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IV.  SWAY  VELOCITY  OBSERVER  DESIGN 


A.  INTRODUCTION 

The  heading  control  law  as  presented  in  the  previous  chapters  is  based  on  full  state 
feedback.  In  reality,  the  sway  velocity  (v)  will  not  be  measured  thus  requiring  an  observer 
to  be  designed  that  can  estimate  its  value.  The  approach  that  will  be  taken  is  to  utilize 
observer  theory  to  obtain  the  necessary  equations  for  a  reduced  order  observer  and  then 
use  the  Matrix-x  software  program  to  obtain  the  necessary  coefficients  for  these  equations. 
The  depth  control  law  as  shown  for  the  runs  thus  far  utilized  a  full  order  observer  [Ref. 
1],  Since  the  speed  control  law  must  be  based  on  full  state  feedback,  sway  velocity  is  the 
only  state  variable  which  requires  an  observer  at  this  point. 


B.  OBSERVER  DESIGN  FOR  LINEAR  AUV 

The  state  space  equations  for  SIS02  is  given  in  Chapter  2  by  Equation  2.9.  The 
state  space  vector  is  rearranged  to  separate  the  measured  from  the  unmeasured  states 
resulting  in  equation  (4.1). 
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To  develop  the  reduced  order  observer,  the  state  space  matrices  will  be  partitioned 
as  shown  in  equation  (4.1)  and  rewritten  as  follows. 
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The  output  system  of  equations  is  then  written  in  the  form  Y=[C]X  as  shown  below. 
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Partitioning  the  C  matrix  and  the  state  vector  Xo  to  separate  the  measured  and 
unmeasured  states  gives: 

[CJ  =  [  J]  (x0l)  =  (J)  (Xoz)  =  (V)  (4.4) 

At  this  point  the  relationship  shown  in  reduced  order  observer  theory  can  be 
systematically  applied  to  complete  the  observer  design  [Ref.  8].  Note,  Xcl  represents  the 
measured  state  variables  and  Xo2  is  the  sway  velocity  that  is  to  be  observed. 
Consequently,  the  following  equation  is  obtained. 

Xo2  =  t?  =  [LL]  ( Y )  +  [ ZZ ]  (4.5) 


In  order  to  better  understand  the  following  relationships  that  are  used  to  find  [LL] 
and  [ZZ],  a  block  diagram  is  of  the  observer  is  shown  in  Figure  13. 
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Figure  13.  Sway  Velocity  Observer  ( Initial  Design  ) 

The  values  of  (FFJ,  IGGJ,  and  [HJ  are  given  by  the  following  equations. 

[FF]  =  [Ao22]  -  (LL]  tCj  [Ao12]  (4.6) 

[ H 3  =  [Bo2 ]  -  [LLIICJIBOJ  (4.7) 

[GG]  =  (  [ Ao21 )  -  [LL]  [CJ  Uoul  )  [CJ  1  (4-8) 
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It  will  be  necessary  to  consider  [LL]  and  [GG]  to  have  two  parts  resulting  in 
[LL]  =  [Ij,l2]  and  [GG]  =  [gg^ggj.  Upon  substituting  into  the  above  relationships  die 
following  equations  are  obtained. 


=  dn 


FF  =  d12 

d22 

(4.9) 

H  =  d13 

dZ3 

(4.10) 

'  h  d21 

-  lz  99z  =  0 

(4.11) 

Notice  that  the  above  values  are  all  scalars  and  that  [LL]  is  the  only  part  of  the 
observer  equation  yet  to  be  determined.  [LL]  will  be  found  using  pole  placement  as 
follows: 

|  S-FF  |  =  |  S  -  d12  -  Jj  d22  |  (4.12) 

Since  I2  is  not  included  in  equation  12  it  can  be  considered  arbitrary  and  assigned 
a  value  of  zero.  The  resulting  equation  for  Ij  =  [LL]  is: 

=  ( -FF  +  d12  )/d22  (4.13) 

It  is  at  this  point  that  Matrix-x  was  used  to  determine  the  coefficients  necessary  to 
calculate  the  observed  value  of  sway  velocity.  In  addition  simulations  were  conducted  to 
compare  the  observed  value  of  sway  velocity  to  the  actual  value.  The  best  results  were 
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obtained  by  choosing  FF  to  have  a  value  of  -2  giving  the  following  equation. 

<?  =  2X  r  +  ZZ  (4.14) 

Where: 

ZZ  =  H  8r  +  ggx  r  +  FF  <? 

FF  =  -2  ggi  =  -57.827  H  -  -15.575  =  -104.850 

The  Matrix-x  system  build  block  diagram  which  was  used  for  the  simulation  is 
shown  in  Figure  14  and  the  entire  autopilot  with  the  observer  included  is  shown  in  Figure 
15. 
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Figure  14.  Reduced  Order  Sway  Velocity  Observer 
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Figure  15.  Autopilot  With  Sway  Velocity  Observer 

The  same  maneuver  simulated  in  Chapter  2  and  shown  in  Figure  5  was  again 
simulated  using  the  estimated  sway  velocity.  A  comparison  of  sway  velocity  and  its 
observed  value  is  given  by  Figure  16.  The  observed  value  is  exactly  the  same  as  the 
actual  showing  that  the  observer  has  been  designed  properly.  The  rest  of  the  run  is 
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included  as  Figure  17  confirming  that  the  autopilot  with  the  reduced  order  observer  is 
operating  properly.  The  next  step  is  to  include  the  sway  velocity  observer  into  the  Fortran 
code  representing  the  nonlinear  AUV. 


Figure  16.  Comparison  of  Modeled  vs  Observed  Sway  Velocity 

(Linear  AUV) 
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C.  APPLICATION  TO  NONLINEAR  AUV 


The  initial  attempt  to  use  the  observer  equation  was  unsuccessful  when  applied  to 
the  nonlinear  AUV.  The  clue  to  the  problem  came  by  noticing  that  the  only  time  the 
observer  worked  well  was  if  the  initial  value  for  the  surge  velocity  (u)  and  the  command 
speed  were  both  6  ft/sec  (the  nominal  linearization  speed).  To  overcome  this  problem  and 
allow  for  a  more  accurate  estimate  of  the  sway  velocity  throughout  its  range  of  operation 
the  Matrix-x  model  of  the  AUV  was  linearized  around  a  variety  of  speeds  and  the  various 
coefficients  were  recorded.  Curve  fitting  techniques  were  then  employed  to  relate  these 
coefficients  to  the  surge  velocity  (u)  so  that  the  nonlinearities  actually  present  in  the  real 
vehicle  could  be  better  accounted  for.  The  resulting  values  for  the  coefficients  are  shown 
in  Appendix  D  along  with  the  equations  which  resulted.  These  equations  were  then 
included  in  the  Fortran  code  and  the  same  simulation  run  was  repeated  with  the  improved 
adaptive  observer  providing  the  estimate  of  sway  velocity.  The  results  are  shown  in 
Figures  18  and  19.  From  the  simulation  it  is  clear  that  the  control  laws  continued  to  woik 
well. 

The  difference  between  the  observed  value  and  the  actual  value  are  shown 
graphically  in  Figure  20.  This  figure  shows  that  the  observed  value  is  a  fair  approximation 
for  v  allowing  accurate  control  of  the  vehicle.  The  accuracy  could  possibly  have  been 
improved  by  linearizing  around  more  speeds  to  get  a  better  set  of  equations  for  the 
coefficients  but  since  the  value  of  v  is  so  small  the  impact  of  assuming  it  to  be  zero  was 
explored  prior  to  attempting  to  refine  the  observer  any  further. 
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Figure  18.  Autopilot  With  Adaptive  Observer  (Nonlinear  AUV) 
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Figure  19.  Autopilot  With  Adaptive  Observer  (Nonlinear  AUV) 


Figure  20.  Comparison  of  Modeled  vs  Observed  Sway  Velocity  (Nonlinear  AUV) 


D.  EFFECT  OF  ASSUMING  SWAY  VELOCITY  NEGLIGIBLE 

Figures  21  and  22  show  the  effect  of  setting  the  sway  velocity  equal  to  zero  and 
commanding  the  same  turn  and  speed  increase  as  shown  in  Figure  16.  The  performance 
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of  the  heading  control  law  is  virtually  the  same!  It  is  a  tribute  to  the  robustness  of  sliding 
mode  control  that  sway  velocity  is  best  handled  as  an  unmodeled  disturbance  than  an 
observed  quantity. 

E.  CONCLUSION 

In  view  of  these  results,  the  autopilot  will  be  designed  upon  the  assumption  that  the 
sway  velocity  equals  zero.  This  will  simplify  the  autopilot  and  increase  the  speed  at  which 
the  calculations  are  performed.  Consequently,  the  problem  of  improving  the  observer 
design  is  eliminated  and  SIS02  is  effectively  reduced  to  a  two  state  feedback  system. 
This  decision  may  nave  to  be  reevaluated  if  disturbances  or  currents  are  added  to  the 
model  that  cause  the  sway  velocity  to  vary  more  than  can  be  handled  by  the  nonlinear 
switching  term  in  the  heading  control  law. 
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Figure  21.  Sway  Velocity  Set  Equal  to  Zero  (Nonlinear  AUV) 
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Figure  22.  Sway  Velocity  Set  Equal  to  Zero  (Nonlinear  AUV) 
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V.  GUIDANCE 


A.  INTRODUCTION 

The  guidance  portion  of  an  AUV  is  the  link  between  the  mission  planner  and  the 
autopilot.  The  mission  planner  decides  the  coordinates  of  the  way  points  the  vehicle  must 
travel  to  and  the  vehicle’s  speed.  The  autopilot,  as  explained  in  the  previous  chapters, 
requires  inputs  of  desired  heading,  depth,  and  speed.  Though  speed  and  depth  can  be 
given  directly  to  the  autopilot,  a  guidance  scheme  that  will  convert  the  x  and  y 
coordinates  of  the  way  point  into  the  required  heading  command  must  be  developed.  The 
method  of  guidance  that  was  selected  is  called  Line  of  Sight  Guidance  (LOS).  In  this 
chapter,  the  basics  of  LOS  are  explained  and  the  critical  aspects  of  an  autopilot  based  on 
LOS  are  examined. 

B.  LINE  OF  SIGHT  GUIDANCE 

1.  Fundamental  Relationships 

Line  of  Sight  Guidance  determines  the  desired  heading  by  using  the  vehicles 
current  position  and  the  coordinates  of  the  next  way  point.  As  its  name  implies,  LOS 
essentially  points  the  AUV  at  the  next  way  point  along  the  line  of  sight  no  matter  where 
the  vehicle  is  located.  Notice  that  some  sensors,  for  example  an  inertial  navigation  system, 
must  be  available  to  determine  the  current  position  of  the  AUV.  Figure  2?  shows  how 
the  desired  heading  (\jrd)  can  be  simply  obtained  from  the  coordinates  of  the  current 
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position  (x,y)  and  the  coordinates  of  the  next  way  point  (x^yj  by  using  the  ATAN 
function. 


(Xd,Yd) 


(X,Y) 


td  =  ATAN  [(Yd-Y)/(Xd-X)] 

Figure  23.  Finding  the  Desired  Heading 

Notice  that  in  Figure  23  a  turn  to  port  is  the  obvious  choice  but  if  the  next 
way  point  had  been  in  the  fourth  quadrant  a  turn  to  starboard  would  have  been  more 
efficient.  This  shows  the  need  to  compare  which  turn  will  allow  the  desired  heading  to 
reached  with  the  least  amount  of  course  correction.  To  understand  how  this  was  done,  it 
should  be  pointed  out  that  y*  will  have  a  range  of  values  -n<y4<n.  To  account  for  the 


cases  when  yd  is  negative,  2n  was  added  to  \yd  to  give  \|/dp.  Next,  a  comparison  of  the 
absolute  value  of  the  angular  difference  between  the  desired  and  the  actual  heading  was 
made.  The  choice  of  yd  or  \j/dp  was  based  upon  which  of  these  gave  the  smaller  course 
correction.  This  value  that  was  called  \|/dai  and  was  value  given  to  the  heading  control 
law  as  the  desired  heading.  Finally,  it  should  be  pointed  out,  that  if  the  term  (Yd--y)  in 
the  heading  control  law  is  positive  a  turn  to  port  will  result.  Consequently,  if  it  is  negative 
the  AUV  will  turn  to  starboard.  An  illustration  of  the  selection  of  the  best  turn  direction 
is  outlined  in  Figure  24. 


t  =  3^/4 
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Figure  24.  Choosing  the  Smallest  Heading  Correction 


2.  Interface  parameters  effecting  performance 

There  are  several  factors  that  impact  the  equation  that  determines  the  desired 
heading  and  consequently  the  effectiveness  of  the  system  as  a  whole.  These  factors  are 


discussed  at  this  point  so  that  the  results  of  the  upcoming  simulations  can  better  be 
appreciated. 

First,  how  often  does  the  autopilot  receive  an  update  as  to  its  current  position 
from  the  navigational  instrumentation  on  board.  This  will  effect  the  accuracy  of  the 
calculation  of  \yd  as  will  the  accuracy  of  die  navigational  instrumentation  itself. 

Next,  a  criteria  must  be  established  for  considering  the  AUV  to  be  considered 
close  enough  to  the  desired  way  point  that  the  next  way  point  should  be  provided  by  the 
mission  planner.  This  has  the  effect  of  aiming  at  a  target  with  the  desired  way  point  at 
its  center  and  will  account  for  the  advance  and  transfer  of  the  AUV.  The  dimensions  that 
are  established  for  this  "zone  of  acceptability"  will  directly  affect  the  amount  by  which 
the  AUV  overshoots  one  way  point  on  its  way  to  the  next. 

The  turning  and  diving  characteristics  of  the  AUV  must  also  be  considered. 
A  way  point  must  be  far  enough  away  that  the  AUV  can  physically  maneuver  to  reach 
it.  On  the  other  hand,  the  more  frequently  the  way  points  are  specified  die  more  control 
the  mission  planner  can  exert  on  where  the  AUV  is  while  in  transit  between  two  way 
points. 

Finally,  the  environment  the  AUV  is  in  must  be  considered.  Factors  such  as 
currents  can  greatly  affect  the  accuracy  with  which  the  AUV  can  be  controlled.  The 
number  of  obstacles  that  must  be  avoided  and  hence  the  greater  need  to  prescribe  a  path 
rather  than  just  a  way  point  will  also  be  an  important  factor  to  consider. 

Keeping  all  these  factors  in  mind,  it  is  clear  that  the  mission  planner,  guidance 
scheme,  and  autopilot  must  ultimately  be  evaluated  as  a  system  with  a  particular  mission 
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rather  than  three  independent  functions.  It  is  in  this  context  that  the  following  results  are 
presented. 

3.  Results 

Run  1  shows  the  versatility  of  the  system  using  LOS  guidance  under  the 
following  nearly  ideal  conditions. 

•  (x,y,z)  position  updates  are  accurate  and  are  provided  every  .1  sec. 

•  Way  points  are  distant  enough  to  be  well  within  the  turning  and  diving  capabilities 
of  the  vehicle. 

•  The  target  radius  is  one  shiplength. 

•  No  disturbances  or  obstacles. 

The  n.:ssion  that  is  envisioned  is  that  of  a  straight  line  transit  followed  by  a 
slow  speed  search.  It  will  require  both  port  and  starboard  turns  and  increases  and 
decreases  in  both  depth  and  speed.  The  way  points  are  identified  in  the  horizontal  plane 
by  crosshairs  and  the  desired  depth  and  speed  are  represented  by  dotted  lines.  Figure  25 
shows  the  results  of  the  simulation.  The  AUV  went  to  each  way  point  that  was  specified 
and  did  so  at  the  required  speed.  The  control  action  of  the  rudder ,  dive  planes,  and  motor 
were  very  efficient.  This  run  has  to  be  considered  to  be  highly  successful  thus  proving 
LOS  to  be  a  valid  guidance  method.  Notice  that  die  mission  ends  with  the  AUV  one 
shiplength  away  from  the  last  way  point  waiting  for  the  new  way  point  to  be  provided 
by  the  planner. 

The  first  parameter  that  was  changed  was  the  time  between  updates  of  the 
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actual  position  of  the  AUV.  The  first  signs  of  a  decline  in  performance  became  evident 
when  the  update  time  reached  5  seconds.  The  results  are  shown  in  Figure  26.  The  control 
was  still  very  successful  which  shows  how  tolerant  LOS  guidance  is  to  inaccuracies  in 
determining  the  AUV’s  position.  Notice  however  that  there  is  an  increase  in  the  control 
effort  for  the  rudder.  The  fluctuations  that  result  whenever  the  update  is  received  are  not 
optimal  so  it  is  recommended  that  the  update  time  be  kept  below  5  seconds. 

The  update  time  and  the  accuracy  with  which  the  AUV’s  position  can  be 
determined  will  become  more  and  more  important  as  the  way  points  become  more  closely 
spaced  in  an  effort  to  achieve  even  greater  control  of  the  transit  path.  Notice  that  these 
way  points  are  approximately  ten  shiplengths  apart. 

The  next  simulation  runs  explored  die  effect  of  the  size  of  the  target  radius. 
The  optimal  target  radius  was  determined  to  be  two  shiplengths.  The  improvement  that 
a  two  shiplength  target  radius  resulted  in  as  compared  to  a  target  radius  of  one  shiplength 
can  be  seen  by  comparing  Figures  25  and  27.  Since  the  next  way  point  is  provided  one 
shiplength  sooner,  the  control  action  required  to  reach  it  can  be  initiated  earlier.  The  old 
way  point  is  still  reached  by  virtue  of  the  momentum  of  the  AUV  but  the  amount  by 
which  the  AUV  overshoots  is  decreased.  This  reduces  the  total  path  length  and 
consequently  improves  the  overall  performance.  A  target  radius  of  two  shiplengths  seems 
to  work  best  since  increasing  it  any  further  would  result  in  missing  die  original  way  point 
by  too  great  a  margin  as  the  AUV  redirects  itself  toward  the  newly  assigned  way  point. 

The  effect  of  the  frequency  with  which  the  way  points  are  given  was  explored  by 
the  next  simulation  run.  In  an  attempt  to  improve  the  performance  of  the  AUV  two 
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additional  way  points  were  included  in  between  those  originally  provided.  This  was  done 
in  an  attempt  to  cause  the  AUV  to  take  more  of  a  straight  line  path  to  the  next  way  point. 
The  additional  way  points  were  in  line  with  the  original  two  and  were  located  about  five 
shiplengths  away.  Five  shiplengths  is  the  turning  diameter  for  this  vehicle  so  the  idea  was 
to  get  a  maximum  turning  effort  initially  and  then  follow  a  straight  line  for  the  second 
part  of  the  path.  The  target  radius  for  this  run  remained  two  ship  lengths  but  the  position 
update  was  1  second  since  the  way  points  are  closer  together.  Figure  28  shows  the  results. 
A  greater  degree  of  control  of  the  AUV  is  attained  and  a  shorter  distance  of  travel  is 
required.  The  improvement  can  be  easily  seen  by  comparing  Figures  27  and  28. 
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Figure  27.  RUN  3,  LOS  Guidance  (Target  Radius  =  2  SL) 
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VI.  HIGH  SPEED  APPLICATIONS 


A.  INTRODUCTION 

The  design  of  the  autopilot  and  LOS  Guidance  system  is  based  on  a  nominal 
operating  speed  of  6  ft/sec.  This  chapter  explores  the  capability  of  the  autopilot  and 
guidance  system  to  successfully  control  the  AUV  during  high  speed  operations.  The  goal 
is  to  control  the  AUV  at  speeds  as  high  as  50  ft/sec  (30  knots)  and  as  low  as  3  ft/sec. 
This  wide  range  of  operating  speeds  will  result  in  an  extremely  large  change  in  the 
vehicle’s  hydrodynamic  coefficients.  Recalling  that  the  control  is  based  on  a  model  that 
was  linearized  around  a  speed  of  6  ft/sec,  it  becomes  clear  that  this  will  be  a  severe  test 
of  the  robustness  of  the  sliding  mode  control  laws. 

To  better  appreciate  the  extent  of  the  unmodeled  behavior,  graphs  of  pitch  and  pitch 
rate  are  included  in  the  simulation  results.  Recall  that  they  were  both  considered  to  be 
zero  during  the  simplification  of  the  equations  of  motion  for  SIS02. 

As  was  mentioned  previously,  the  depth  control  law  has  been  based  on  observed 
values  of  pitch  and  pitch  rate  up  to  this  point.  This  is  advantageous  for  a  small  vehicle 
that  does  not  have  enough  room  for  the  instrumentation  necessary  to  measure  these 
values.  For  an  AUV  capable  of  a  30  knot  operating  speed  however,  it  is  assumed  that 
there  will  be  instrumentation  onboard  to  provide  these  measurements.  Consequently,  the 
simulation  will  be  done  assuming  full  state  feedback  with  the  exception  of  the  sway 
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velocity.  Sway  velocity  will  still  be  given  a  value  of  zero  and  handled  as  a  disturbance 
as  was  explained  in  Chapter  4. 

B.  AUTOPILOT  SIMULATION  RESULTS 

The  first  set  of  simulation  runs  test  the  ability  of  the  autopilot  to  achieve  the  desired 
heading,  speed,  and  depth.  The  capability  of  the  LOS  guidance  system  will  be 
demonstrated  later  once  the  autopilot  has  been  verified. 

1.  Run  1 

The  conditions  for  Run  1  are  as  follows: 

•  Command  speed  18  FPS. 

•  Command  depth  2  Shiplengths 

•  Command  heading  90  Degrees 

•  Integration  time  step  .01  Sec. 

This  run  tested  the  ability  of  the  autopilot  to  operate  at  a  speed  three  times 
greater  than  it  was  designed  for.  Before  satisfactory  results  could  be  attained  at  higher 
speeds,  the  integration  time  step  had  to  be  decreased.  The  relationship  between  the  surge 
velocity  and  the  integration  time  step  is  found  by  expressing  it  in  a  nondimensional  time 
as  shown  in  Equation  6.1. 

NDb t  -  u  b.t/L  (6.1) 
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This  relationship  shows  that  if  the  nondimensional  time  step  is  to  remain 
constant,  the  time  step  used  within  the  simulation  must  decrease  as  the  speed  increases 
to  obtain  the  correct  results.  By  referring  back  to  the  initial  work  at  slow  speeds,  the 
value  for  the  nondimensional  time  step  was  found  to  be  .1.  Based  upon  Equation  6.1,  an 
integration  time  step  of  .01  sec  was  chosen  for  the  surge  velocity  of  18  ft/sec. 

The  results  as  shown  in  Figures  29  and  30  are  outstanding.  The  autopilot  operates  even 
better  at  high  speed.  All  desired  values  are  attained  with  a  minimal  amount  of  control 
effort  and  with  no  modification  of  the  control  laws.  The  autopilot  is  now  ready  to  be 
verified  using  the  boundaries  of  its  design  envelope. 

2.  Run  2 

The  integration  time  step  was  decreased  to  .005  seconds  for  this  run  at  50 
ft/sec  to  be  consistent  with  keeping  the  nondimensional  time  step  constant.  In  addition  the 
Auv  is  given  an  initial  surge  velocity  of  50  ft/sec  rather  than  starting  from  rest  to  show 
the  effect  of  a  high  speed  turn.  The  remaining  characteristics  of  this  run  are  the  same  as 
Run  1.  The  results  are  shown  in  Figures  31  and  32. 

Again  the  results  are  very  good  with  no  problems  evident.  This  result  is  a  most 
impressive  verification  of  the  robustness  of  sliding  mode  control. 

3.  Run  3 

Run  3  is  included  to  show  that  the  autopilot  is  also  capable  of  slow  speed 
operations  at  3  ft/sec.  An  integration  time  step  of  .1  seconds  is  adequate  for  this  speed. 
Figures  33  and  34  show  that  the  AUV  is  more  sluggish  at  the  slower  speed  but  the 
control  laws  have  no  problem  attaining  the  commanded  heading,  speed,  and  depth. 
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Figure  29.  Run  1,  High  Speed  Maneuver  (18  ft/sec) 
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Figure  31.  Run  2,  Maximum  Speed  Maneuver  (50  ft/sec) 
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Figure  33.  Run  3,  Slow  Speed  Maneuver  (3  ft/sec) 
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Figure  34.  Run  3,  Slow  Speed  Maneuver  (3  ft/sec) 
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4.  Conclusion 


The  autopilot  has  been  verified  to  have  the  robustness  necessary  to  operate 
within  a  speed  range  of  3  ft/ sec  to  50  ft/sec.  The  integration  time  step  can  be  set  at  .005 
sec  for  all  values  of  operating  speed  or  it  can  be  related  to  a  constant  nondimensional 
time  step  of  .1  using  Equation  6.1.  The  advantage  of  varying  die  time  step  is  that  the 
calculations  are  performed  faster  for  the  slow  speed  runs  where  the  time  step  can  have 
a  larger  value. 


C.  LOS  GUIDANCE  SIMULATIONS 

The  final  link  in  establishing  that  the  AUV  is  ready  for  high  speed  operations  is  to 
show  that  LOS  Guidance  can  still  steer  it  through  the  desired  way  points  at  a  speed  of 
50  ft/sec.  This  will  be  demonstrated  by  commanding  the  same  basic  maneuver  performed 
in  Chapter  5.  To  decrease  the  time  needed  to  accelerate  to  the  new  higher  speeds,  the 
simulation  will  start  with  the  AUV  already  at  50  ft/sec  when  the  first  way  point  is 
provided.  The  way  points  themselves  will  be  further  apart  as  is  appropriate  for  high  speed 
travel. 

1.  Run  4 

The  conditions  for  Run  4  were  as  follows: 


•  Commanded  speed 

•  Integration  time  step 

•  Target  radius 

•  Navigation  update  time 


50  Ft/sec. 

.005  Sec. 

2  Shiplengths 
.005  Sec. 
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The  results  of  the  run  are  shown  in  Figure  35.  Though  the  AUV  passed 
through  all  of  the  way  points,  the  control  of  speed  is  not  satisfactory.  In  exploring  the 
speed  control  law  for  SISOl,  it  was  determined  that  the  nonlinear  switching  term  was  not 
strong  enough  to  call  for  the  necessary  motor  rpm  to  maintain  the  speed  characteristics 
on  the  sliding  surface. 

2.  Run  5 

For  Run  5  the  value  of  the  coefficient  of  the  switching  term  was  increased  to 
4000  thus  enabling  the  switching  term  itself  to  call  for  enough  rpm  to  result  in  a  50  ft/sec 
speed.  With  this  modification,  the  speed  control  law  for  SISOl  becomes: 

(6.2) 

RPM  =  -4000.0  satagnl  +  83.33  u 

With  this  new  speed  control  law  in  place,  the  same  maneuver  was  again 
commanded.  The  results  are  shown  in  Figure  36.  The  control  of  speed  is  now  much  better 
and  the  AUV  still  passes  through  the  desired  way  points.  Notice  that  when  the  desired 
speed  is  not  attained,  the  control  now  calls  for  the  maximum  effort,  which  is  5000  rpm, 
from  the  propulsion  motor.  In  order  to  improve  speed  control  any  further,  a  larger 
propulsion  system  would  have  to  be  utilized. 

3.  Run  6 

This  run  explores  the  maximum  Navigation  update  time  interval  that  can  be 
tolerated  at  high  speeds  before  a  decrease  in  performance  is  detected.  Various  nuts  were 
conducted  with  the  first  signs  of  trouble  occurring  when  the  navigation  update  time  was 
1  sec.  The  results  for  this  run  are  shown  in  Figure  37.  The  need  for  a  shorter  Navigation 
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update  time  is  to  be  expected  since  the  AUV  travels  further  at  high  speeds  during  the 
update  interval  thus  causing  it  to  be  further  away  from  the  last  known  position  used  by 
the  control  law. 

4.  Conclusion 

LOS  Guidance  works  as  well  at  50  ft/sec  as  it  did  at  3  ft/sec  for  the  runs 
shown  in  Chapter  5.  The  only  modifications  that  were  required  were  to  increase  the 
strength  of  the  switching  term  in  the  speed  control  law  and  decrease  the  integration  time 
step  and  navigation  update  time.  The  fact  that  only  the  speed  control  law  had  to  be 
modified  is  consistent  with  the  fact  that  the  control  surfaces  become  more  effective  at 
higher  speeds  resulting  in  more  response  to  a  given  control  signal.  To  increase  the  speed, 
on  the  other  hand,  requires  more  motor  rpm  thus  causing  the  speed  control  law  to  be 
modified  consistent  with  the  larger  propulsion  system  that  would  have  to  be  installed  to 
achieve  the  higher  operating  speed. 
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VII.  EFFECT  OF  CURRENT  ON  LOS  GUIDANCE 


A.  INTRODUCTION 

This  chapter  investigates  the  effect  of  current  on  the  autopilot’s  ability  to  reach  a 
series  of  way  points  using  LOS  Guidance.  The  speed  during  these  runs  will  be  4  ft/sec 
which  is  consistent  with  the  operating  speed  of  the  AUV  under  construction  at  NPS.  The 
first  runs  attempt  to  reach  a  single  way  point  with  the  current  coming  from  various 
directions.  The  next  simulations  will  show  the  effect  of  the  strength  of  the  current.  The 
last  set  of  simulations  show  how  the  presence  of  a  strong  current  can  effect  a  path  made 
up  of  multiple  way  points. 

B.  EFFECT  OF  CURRENT  DIRECTION 

X.  Run  1 

To  see  just  the  effect  of  the  direction  of  the  current,  a  single  way  point  at 
(10,10,0)  was  chosen  and  a  constant  speed  of  4  ft/sec  was  commanded  for  the  AUV. 
Figure  38  shows  the  path  that  resulted  when  no  current  was  present.  Notice  that  a  new 
graph  of  the  distance  the  AUV  is  away  from  the  next  way  point  (DAW AY)  has  been 
added.  The  target  radius  was  set  at  .01  shiplengths  to  allow  the  vehicle  to  get  as  close  as 
possible  to  the  prescribed  way  point.  This  represents  the  standard  to  which  all  other  runs 
can  be  compared. 
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2.  Run  2  and  Run  3 


These  runs,  shown  in  Figure  39,  demonstrate  the  effect  of  a  2  ft/sec  current. 
This  current  is  50  %  of  the  operating  speed  of  the  AUV  which  should  be  considered  to 
be  an  extreme  case.  Both  runs  are  shown  in  Figure  39.  For  Run  2  the  current  was  from 
left  to  right  and  for  Run  3  the  current  was  right  to  left.  Notice  that  the  AUV  still  reaches 
the  way  point  in  both  cases  but  is  swept  away  from  the  no  current  path.  Notice  too  that 
in  both  cases  the  AUV  has  a  final  approach  which  is  against  the  current. 

3.  Run  4  and  Run  5 

For  Run  4  the  current  direction  was  from  top  to  bottom.  For  Run  5  it  was 
from  bottom  to  top.  In  both  cases  the  final  approach  once  again  is  against  the  current,  see 
Figure  40.  In  addition  the  miss  distance  was  the  largest  when  the  current  comes  from 
bottom  to  top.  It  should  be  remembered  however,  that  the  target  radius  that  will  be  used 
during  the  actual  operation  is  two  shiplengths.  Since  the  AUV  was  able  to  get  within 
approximately  1.5  shiplengths,  this  run  should  be  considered  a  hit  also. 

4.  Conclusion 

The  AUV  will  reach  the  prescribed  way  point  under  extremely  adverse  current 
conditions.  The  problem  however  is  that  its  path  deviates  from  the  near  straight  line  path 
of  the  no  current  case  to  the  point  that  it  may  be  unsuitable  for  operation  in  an 
environment  which  requires  a  high  degree  of  control  over  the  path.  The  other  problem  to 
address  is  that  the  AUV  will  approach  the  way  point  against  the  current  which  may  be 
undesirable  considering  the  position  of  the  next  way  point  along  its  path. 
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C.  ADAPTATION  OF  STEERING  CONTROL 


For  the  runs  shown  in  Figures  39  and  40  the  value  for  the  x  and  y  position  of  the 
AUV  are  calculated  by  the  following  equations  where  the  subscript  c  denotes  current. 

it  =  uc  +  u  costjr  -  v  sintjr 


y  =  vc  +  u  sintjr  +  v  sintjr 

Recall  too  that  the  desired  heading  is  calculated  using  Equation  7.2. 

tjrd  =  ATAN[  iy~yd)  /  ( x-xd )  ] 


(7.1) 


(7.2) 


Recall  that  at  steady  state  the  desired  heading  and  the  actual  heading  are  equal.  Therefore, 
at  steady  state,  with  a  desired  way  point  at  (0,0)  for  simplicity.  Equation  (7.2)  can  be 
substituted  into  Equation  (7.1)  which  upon  integration  yields  the  result  shown  as  Equation 
(7.3). 

ijrd  =  ATAN  (vc/uc)  (7.3) 


Equation  (7.3)  shows  that  the  desired  heading  at  steady  state  ultimately  is  parallel  to  the 
direction  of  the  current.  This  result  is  verified  by  the  simulation  results  shown  in  Figures 
39  and  40.  Notice  that  if  the  vertical  component  of  the  current  is  zero  as  is  the  case  for 
Figure  39  the  desired  heading  becomes  0  or  180  degrees.  Correspondingly,  if  the 
horizontal  component  of  the  current  is  zero,  the  desired  heading  is  +  «t  -  °0  degrees  as 
shown  in  Figure  40. 
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Once  an  understanding  of  the  above  relationships  was  attained,  the  next  step  was 
to  modify  the  control  law  to  attain  tighter  control  over  path.  This  was  done  by  specifying 
a  desired  heading  that  compensates  for  the  direction  of  the  current  rather  than  being 
controlled  by  it.  To  do  this,  recall  that  the  equation  for  the  sliding  surface  for  SIS02  is 
given  by  Equation  (2.16)  which  is  repeated  here  as  follows: 

o  =  s1(i|r  -  fd)  +  s2(v)  +  s3(r)  (7.4) 

By  considering  the  simple  case  where  the  AUV  is  commanded  to  travel  along  the  x  axis, 
a  modification  to  the  sliding  surface  can  be  attained.  For  this  simple  path  the  desired 
heading  is  zero.  In  addition  the  speed  in  the  y  direction  must  also  be  zero.  This 
observation  coupled  with  Equation  (7.1)  gives  the  relationship  shown  in  Equation  (7.5) 
that  can  be  used  to  find  the  steady  state  error  that  must  exist  to  compensate  for  the  effect 
of  the  current. 

i|fs  =  -  ASIN(vJu)  (7.5) 


This  result  requires  that  the  actual  heading  of  the  AUV  be  composed  of  two  parts  as 
shown  in  Equation  (7.6). 

Y  «  ♦<,  +  <7*6> 

Recalling  that  the  sway  velocity  v  has  been  estimated  to  be  zero  and  that  r  is  zero  at 
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steady  state,  the  sliding  surface  equation  shown  in  Equation  (7.4)  becomes: 

o  =  sx  Hr  =  s1  4rs 

Notice  that  the  sliding  surface  equation  will  not  be  zero  at  steady  state  unless  another 
teim  equal  in  magnitude  but  opposite  in  sign  is  added  to  the  sliding  surface  equation.  It 
is  by  this  reasoning  that  the  equation  for  the  sliding  surface  for  SIS02  was  modified  to 
become: 

o  =  sx(i}r-i|rd)  +  s2v  +  s3r  +s1ASIW(  vc/u) 

This  additional  term  has  the  effect  of  introducing  a  steady  state  error  for  heading  that 
exactly  offsets  the  effects  of  the  current  and  improves  the  path  keeping  characteristics  of 
the  AUV.  It  should  be  pointed  out  that  this  equation  relies  on  perfect  knowledge  of  the 
current  velocity  allowing  the  component  of  the  current’s  velocity  that  is  perpendicular  to 
the  AUV’s  straight  line  path  between  consecutive  way  points  to  be  determined. 

The  improvement  which  results  is  shown  by  rerunning  the  same  simulations 
discussed  at  the  beginning  of  this  chapter  using  the  modified  sliding  surface  for  SIS02. 
The  improvement  is  easily  seen  in  Figures  41, 42, 43, and  44  where  the  old  results  are  on 
the  left  and  the  new  results  are  on  the  right.  Based  upon  this  comparison,  it  can  be 
concluded  that  LOS  Guidance  can  be  highly  successful  at  controlling  an  AUV  in  a  high 
current  environment  if  the  velocity  of  the  current  is  well  known.  This  shifts  the  burden 
of  tight  control  over  the  path  to  the  design  of  an  accurate  estimator  for  current  velocity. 
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Such  an  observer  will  not  be  included  in  this  work  but  the  following  simulation  is 
included  to  give  an  indication  of  the  impact  an  inaccurate  estimate  of  current  would  have. 
The  current  was  given  a  value  of  2  ft/sec  from  bottom  to  top.  The  estimated  value  was 
1  ft/sec  from  bottom  to  top.  Figure  45  shows  that  the  result  is  still  improved  from  the 
case  where  no  compensation  for  current  was  included.  From  this  it  can  be  concluded  that 
even  an  inperfect  estimate  of  the  currents  velocity  will  result  in  some  improvement  in 
the  AUV’s  path.  The  better  the  estimate  the  more  improvement  that  results. 

D.  MULTIPLE  WAY  POINT  PATHS 

The  ultimate  test  for  the  autopilot  using  LOS  Guidance  is  to  show  that  it  can  reach 
a  series  of  way  points.  The  same  path  used  through  out  this  work  is  repeated  here  one  last 
time  to  demonstrate  that  the  AUV  will  travel  through  a  series  of  way  points  even  in  the 
presence  of  a  current  that  has  a  magnitude  of  1.4  fi/sec  in  a  direction  45  degrees  away 
from  the  origin.  The  result  is  shown  in  Figure  46  which  can  be  compared  to  the  same  run 
done  without  a  current  present  in  Figure  47.  The  results  show  that  LOS  Guidance  based 
on  a  heading  control  autopilot  that  has  been  modified  to  account  for  the  presence  of  a 
current  is  very  capable  of  guiding  the  AUV  through  the  prescribed  way  points. 
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Figure  42.  Original  Autopilot  (Left)  vs  Modified  (Right) 
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Figure  44.  Original  Autopilot  (Left)  vs  Modified  (Right) 
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Figure  45.  Effect  of  an  Inaccurate  Current  Velocity  Estimate 
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Figure  46.  Traveling  to  Way  Points  in  a  Strong  Current 
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Vin.  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  CONCLUSIONS 

The  following  are  the  major  conclusions  that  can  be  drawn  from  this  study. 

•  The  control  laws  for  speed,  heading,  and  depth  were  all  developed  independently, 
and  yet  their  individual  control  actions  did  not  interfere  with  each  other. 

•  The  control  laws,  based  on  a  simple  linear  model,  were  able  to  control  the  more 
complex  nonlinear  model  of  the  SDV  MARK  9  vehicle  by  virtue  of  the  nonlinear 
switching  terms,  associated  with  Sliding  Mode  Theory,  that  compensated  for  any 
unmodeled  behavior  and  enhanced  robustness. 

•  In  developing  the  steering  control  autopilot,  sway  velocity  is  best  handled  as  a 
disturbance  rather  than  an  observed  quantity. 

•  LOS  Guidance  provides  an  effective  means  of  interpreting  the  position  coordinates 
of  the  way  points  provided  by  the  mission  planner  into  commands  for  heading  that 
can  be  executed  by  the  autopilot.  An  inherent  characteristic  of  LOS  Guidance  is  that 
it  enhances  the  robustness  of  the  system  as  a  whole.  In  addition,  tighter  control  of 
path  can  be  attained  by  specifying  the  way  points  more  frequently. 

•  The  robustness  of  this  combination  of  Sliding  Mode  Control  and  LOS  Guidance  is 
demonstrated  by  the  3  ft/sec  to  50  ft/sec  speed  envelope  over  which  the  autopilot 
operated  effectively  even  though  the  linearization  that  produced  the  simple  linear 
model  upon  which  the  control  laws  were  based  was  done  about  a  nominal  operating 
speed  of  6  ft/sec. 

•  LOS  Guidance  can  be  easily  modified  to  take  advantage  of  known  or  observed 
current  velocity  thus  allowing  the  vehicle  to  operate  in  strong  currents. 
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B.  RECOMMENDATIONS 


The  following  actions  should  be  taken  to  fully  utilize  the  research  done  for  this 
thesis. 

•  Verify  simulation  results  using  the  actual  AUV  currently  under  construction  at  NPS. 

•  Contrast  this  autopilot  using  LOS  Guidance  to  other  techniques  for  controlling 
AUVs,  such  as  minimizing  cross  track  error,  to  ensure  the  best  method  is  ultimately 
adopted. 

•  Design  an  observer  that  can  determine  the  velocity  of  the  current  that  the  AUV  is 
operating  in. 
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APPENDIX  A 


//*  * 

//*  AUV  AUTOPILOT  EXEC  PROGRAM  * 

//*  * 

//*  THIS  PROGRAM  CALCULATES  ALL  VALUES  NEED  TO  SIMULATE  * 

//*  SPEED  AND  HEADING  CONTRC',  FOR  A  LINEAR,  FULL  STATE  * 

//*  FEEDBACK,  MODEL  OF  THE  AUV.  * 

//*  * 


//THIS  PROGRAM  COMPUTES  THE  S  COEFFICIENTS  FOR  2  SISO  SYSTEMS. 

//  heading  is  entered  in  degrees  and  converted  to  radians  in  the  program 


//  *********************  SYMBOL  IDENTIFICATION  *********************** 

//  ubar-  nominal  operating  speed  upon  which  the  linearization  is  based  (ft/sec). 
//  nbar-  motor  RPM  that  results  in  ubar. 

//  L  -  length  of  SDV-9  (ft). 

//  mass  -  mass  of  SDV-9  (slugs). 

//  Iz  -  moment  of  inertia  of  the  SDV-9  (ft  lb  sec2). 

//  den  -  fluid  density  (slugs/ft3). 

//  Hd  -  desired  heading  (degrees). 

//  ud  -  desired  surge  speed  (ft/sec). 

//**********************  ASSIGNMENT  OF  CONSTANT  VALUES  ****************** 

ubar-6 ; 
nbar-500 ; 

L-17.4; 
mass- 37 2 ; 

Iz-10000; 
den-62 . 4/32 . 2 ; 
kvi s-1 . 082D-5 ; 

//FLOW  PARAMETERS 
Yr- .  0297 ; 

Yv—  .  0931  ; 

Nrd-- . 0034 ; 

Ydr- . 0273 ; 

Yvd— .0555; 

Nr— .0164; 

Nv—  .00742; 

Ndr— .0129; 

Xud—  .0076; 

Nvd- . 0012 ; 

Yrd- . 0012 ; 

CDo-. 00385  +  1 .296D-17*(ubar*l/kvis  -1.2D-7)**2; 


//**  DEFINATION  OF  SYMBOLS  REPRESENTING  CONVIENENT  GROUPING  OF  TERMS  ** 
ALPHA-2 *den*L** 2 *CDo/( 2*mass-den*L* * 3*Xud ) ; 
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// 

// 

// 

// 

// 


*  CALCULATION  OF  A  AND  B  MATRICI ES  FOR  SISOl  WHERE  X-luJ'  * 


Al  -  -ALPHA*ubar; 

B1  -  ALPHA*. 012**2*nbar; 


// 

// 

// 

M-0 . 0*ones ( 2,2); 
C«0 . 0*ones ( 2,3); 


DECOUPLING  THE  LATERAL  AND  YAW  EQUATIONS  OF  MOTION 


M( 1 , 1 )-mass-den*L**3*Yvd/2 ; 

M(  1 , 2  )”-den*L**4 * Yrd/2 ; 

M(  2 ,  1 )— den*L**4*Nvd/2; 

M( 2, 2 )-Iz-den*L**5*Nrd/2 ; 

//C( 1 , 1 )»den*L**3*Yr*ubar/2 ; 

C( 1 , 1 )«den*L**3*Yr*ubar/2  -  mass*ubar; 
C( 1 , 2 )-den*L**2*Yv*ubar/2 ; 

C(  1 , 3)-den*L**2*Ydr*ubar**2/2; 

C( 2,1 )-den*L**4*Nr*ubar/2; 

C( 2 , 2 )-den*L*  *  3*Nv*ubar/2 ; 

C( 2 , 3 ) -den*L**3*Ndr*ubar **2/2 ; 


// 

//  * 

//  *  CALCULATION  OF  A  AND  B  MATRICI ES  FOR  SIS02 

//  * 

// 


WHERE  X«[Heading,v, r ) '  * 

* 


D-inv(M) *C; 

A2«*  [  0,0,l;0,d(l,2)  ,d(  1,1)  ;  0  ,  d  (  2 , 2  )  ,  d  (  2 , 1  )  )  ; 
B2— ( 0;d( 1,3) ;d(2, 3)  )  ; 


//  *******************  CALCULATION  OF  S1,S2,S3,S4 

inquire  atasql 
inquire  atasq2 

inquire  poles2 

kc2«  poleplace ( A2 , B2 , poles2 ) ; 

AC2-A2-B2*kc2; 

J  evec2 , eval2 )-eig( AC2 ' ) ; 

51- 1; 

52- evec2 ( 1,1); 

53- evec2( 2,1); 

54- evec2( 3,1); 
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//  ******************************************************************** 

//  *  DETERMINING  GAIN  VALUES  FOR  MATRIXx  BLOCK  DIAGRAM.  * 

//  ******************************************************************** 

//  CONVENIENT  GROUPING  OF  TERMS: 

DDl-COUD* .012**2*nbar; 

DD2-S3*d(l,3)+S4*d(2,3); 

Natal— atasql/DDl  ; 

Nata2— atasq2/DD2 ; 

NN1-S1*A1/DD1 ; 

NN2-(S3*d(l,2)  +  S4*d( 2 , 2  ) )/DD2 ; 

NN3-(S2+  S3*d ( 1 , 1 )  +  S4*d(2,l) )/DD2; 

d2 3-d (2,3); 
d21»d( 2,1); 
dl3-d(l,3); 
d22-d ( 2,2); 
dl2-d( 1,2); 
dl l-d( 1,1); 

//  ********************************************************************** 
//  *  SIMULATION  .  * 

//  ********************************************************************** 
inquire  tsim 
tstep- . 1 ; 
inquire  ud 
inquire  HdDEG 
Hd  -  HdDEG *6. 28/360; 

T-(0:tstep:tsim]'; 
ud-ud*ones  ( T) ; 

Hd-Hd*ones(T) ; 

U» | ud , Hd  J ; 

Y  -  SIM(T,U) ; 

//  ******************************************************************* 

//  *  GRAPHICAL  DISPLAY  OF  RESULTS  * 

//  ******************************************************************* 

HHD-[Y( : , 2 ) * 360/6 . 28  Hd*360/6 . 28 } ; 

UUD-[Y(  :  ,1)  ud]; 

PLOT(T,Y( : ,3) , 'UPPER  LEFT  XLABEL/T/  Y LAB EL/S I GMAl /TITLE/  ATASQl- . 6  ... 
BAND-1 FPS/' ) 

PLOT(T,Y( : ,5) , 'UPPER  RIGHT  XLABEL/T/  YLABEL/SIGMA2/TITLE/ATASQ2- . 4  POLES. 
-10. -.4, -.42]  BAND- . 1 RAD . / ' ) 

PLOT(T,Y( : ,6) , 'LOWER  RIGHT  XLABEL/T/  Y LAB EL/RUDDER/TITLE/  STEERING  . 

(MAX  DEFLECTION-. 4RAD)/' ) 

. PLOT(T,Y( : ,4) , 'LOWER  LEFT  XLABEL/T/  YLABEL/RPM/TITLE/  PROPULSION. 

(MAX  RPM-  500RPM )/' ) 


PLOT(T,HHD, 'UPPER  RIGHT  XLABEL/T/  YLABEL/H  &  HD/TITLE/  HEADING  ... 

(Hd  -  90  DEG)/* ) 

PLOT(T,UUD, 'UPPER  LEFT  XLABEL/T/  YLABEL/  U  S  UD/TITLE/  SPEED  ... 

(Ud  -  4.0  FPS)/' ) 

//exec( ' RUNSUPERGl . X ' ) 
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APPENDIX  B 


Q  *************************************************************** 

c  *  * 

C  *  NONLINEAR  AUV  MODEL  /  STERN  PLANE  AND  BOW  PLANE  SEPARATED  * 
C  *  * 


c 

* 

VARIABLE  DECLARATION 

11 

INPUTS 

280 

* 

c 

* 

CONSTANTS 

80 

PROPULSION  MODEL 

300 

* 

c 

* 

INITIAL  CONDITIONS 

136 

OUTPUTS 

452 

* 

c 

* 

MASS  MATRIX 

230 

INTEGRATION 

475 

* 

c 

* 

INVERT  MATRIX 

268 

CONTROL  LAWS 

500 

* 

C  *************************************************************** 

REAL  AW{ 82,82) 

REAL  MASS , LATYAW,NORPIT 
REAL  MM(6,6),G4(4),GK4(4),BR(4),HH{4) 

REAL  B(6,6) , BB ( 6 , 6  ) 

REAL  A( 12 , 12 ) ,  AA (12,12) , I NDX (100) 

REAL  XPP  , XQQ  ,XRR  ,XPR 
REAL  XUDOT  ,XWQ  ,XVP  , XVR 
REAL  XQDS  , XQDB  , XRDR  ,XVV 
REAL  XWW  , XVDR  ,XWDS  ,XWDB 
REAL  XDSDS , XDBDB  , XDRDR  , XQDSN 
REAL  XWDSN  ,XDSDSN 

REAL  TIME , S , EITA, UBAR , UHAT , COMZ , BAR , S IM , DE , SAT , VHAT , ZZOBS 
REAL  SSI , SS2 , UD , XD , YD , TD , TNWP , XA , YA , HD , HDMDEG , DAWAY , SATSGNl 
REAL  SATSGN2 , ZZOBSDOT , NEARMI SS , ALPHA , VCC , XDl , YDl , XD2 , YD2 , DY , DX 
REAL  NAVUPDATE , TNAV , TARGET , FF , GG , HHH , LLL , HDP , HDM , LDAWAY 
INTEGER  DV 

LATERAL  HYDRODYNAMIC  COEFFICIENTS 

REAL  YPDOT  , YRDOT, YPQ  , YQR 
REAL  YVDOT  , YP  , YR  , YVQ 
REAL  YWP  , YWR  ,YV  , YVW 
REAL  YDR  , CDY 

NORMAL  HYDRODYNAMIC  COEFFICIENTS 

REAL  ZQDOT  ,ZPP,ZPR  ,ZRR 
REAL  ZWDOT  , ZQ  , ZVP  ,ZVR 
REAL  ZW  ,ZVV  , ZDS  , ZDB 
REAL  ZQN  , ZWN  ,ZDSN  , CDZ 
REAL  ZHADOT, ZHAT 

ROLL  HYDRODYNAMIC  COEFFICIENTS 

REAL  KPDOT  , KRDOT  ,KPQ  , KQR 
REAL  KVDOT  ,  KP  , KR  , KVQ 
REAL  KWP  ,  KWR  , KV  , KVW 

R"AL  KPN  ,  KDB 

PITCH  HYDRODYNAMIC  COEFFICIENTS 

REAL  MQDOT  ,MPP  .MPR.MRR 
REAL  MW DOT  ,  MQ  ,MVP  , MVR 
REAL  MW  ,  MW  ,  MDS  ,  MDB 
REAL  MQN  ,  MWN  , MDSN 
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REAL  QHADOT , QHAT , THADOT , THAT 

YAW  HYDRODYNAMIC  COEFFICIENTS 

REAL  NPDOT , NRDOT , NPQ  , NQR 
REAL  NVDOT  ,  NP  , NR  , NVQ 

RTAL  NWP  ,  NWR  , NV  ,  NVW 
REAL  NDR 

MASS  CHARACTERISTICS  OF  THE  FLOODED  VEHICLE 

REAL  WEIGHT  ,  BOY  ,VOL  ,XG 

REAL  YG  ,  ZG  ,XB  ,Z8 

REAL  IX  ,  IY  ,IZ  ,IXZ 

REAL  IYZ  ,  I XV  , YB 

REAL  L  ,  RHO  ,G  ,NU 

REAL  AO  , RPROP  ,NPROP  ,  XlTEST 

REAL  DEGRUD  , DEGSTN 

COMMON  /BLOCK1/  F(12),  FP(6),  XMMINV(6,6),  UCF(4) 

INTEGER  N, I A, IDGT , IER, LAST ,J,K,M,JJ,KK,I 
REAL  WKAREA( 54 ) ,  X(12) 

RUDDER  COEFFICIENTS 

PARAMETER  (  DSMAX-  -0.175) 

LONGITUDINAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER ( XPP  -  7.E-3  , XQQ  -  -1 . 5E-2  , XRR  -  4.E-3  , XPR  -7.5E-4, 

&  XUDOT— 7.6E-3  , XWQ  -  -2.E-1  , XVP  -  -3.E-3  , XVR  -  2.E-2, 

&  XQDS-2.5E-2  ,XQDB=-2 . 6E-3  ,XRDR-  -l.E-3  , XVV  -5.3E-2, 

&  XWW  -1.7E-1  ,XVDR«1.7E-3  , XWDS-4 . 6E-2  ,XWDB-  l.E-2, 

&  XDSDS-  -l.E-2  , XDBDB"  -8.E-3  ,XDRDR-  -l.E-2  ,XQDSN-  2.E-3, 

&  XWDSN-3 . 5E-3  , XDSDSN"  -1.6E-3  ) 

LATERAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETERf  YPDOT-1 . 2E-4  , YRDOT-1 . 2E- 3  , YPQ  -  4.E-3  , YQR  — 6.5E-3, 

6  YVDOT— 5.5E-2  , YP  -  3.E-3  , YR  =  3.E-2  , YVQ  -2.4E-2, 

&  YWP  -2.3E-1  , YWR  --1.9E-2  , YV  -  -l.E-1  , YVW  -6.8E-2, 

&  YDR  -2.7E-2  , CDY  -3.5E-1) 

.NORMAL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER (ZQDOT— 6. 8E- 3  ,ZPP  -1.3E-4  ,ZPR  -6.7E-3  ,ZRR  — .7.4E-3, 
&  ZWDOT— 2.4E-1  , ZQ  — 1.4E-1  ,ZVP  --4.8E-2  ,ZVR  -4.5E-2, 

&  ZW  -  -3.E-1  ,ZVV  — 6.8E-2  ,ZDS  — 7.3E-2  ,ZDB  — 2.6E-2, 

t  ZQN  — 2.9E-3  , ZWN  — 5.1E-3  ,ZDSN«  -l.E-2  , CDZ  -  1.0) 

ROLL  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER (KPDOT-  -l.E-3  , KRDOT--3 . 4E-5  ,KPQ  — 6.9E-5  , KQR  -1.7E-2, 

5  KVDOT-1 . 3E-4  ,  KP  — 1.1E-2  , KR  — 8.4E-4  , KVQ— 5.1E-3, 

6  KWP  — 1.3E-4  ,  KWR  -1.4E-2  , KV  -3.1E-3  , KVW  — 1.9E-1, 

(  KPN  — 5.7E-4  ,  KDB  -  0.0  ) 

PITCH  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER ( MQDOT— 1 . 7 E- 2  ,MPP  -5.3E-5  ,MPR  -  5.E-3  , MRR  — 2.9E-3, 

&  MWDOT— 6.8E-3  ,  MQ  --6.8E-2  ,MVP  -1.2E-3  ,MVR  -1.7E-2, 
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£  MW  -  l.E-1  ,  MW  --2.6E-2  , MDS  — 4.1E-2  , MDB  -6.9E-3, 

£  MQN  — 1.6E-3  ,  MWN  — 2.9E-3  , MDSN  — 5.2E-3) 

YAW  HYDRODYNAMIC  COEFFICIENTS 

PARAMETER  (NPDOT— 3 . 4E-5  , NRDOT-- 3 . 4E-3 , NPQ  — 2.1E-2  , NQR  -2.7E-3, 
£  NVDOT-1.2E-3  ,  NP  --8.4E-4  , NR  --1.6E-2  , NVQ  -  -l.E-2, 

£  NWP  --1.7E-2  ,  NWR  -7.4E-3  ,  NV  --7.4E-3  , NVW  — 2.7E-2, 

£  NDR  — 1.3E-2) 


MASS  CHARACTERISTICS  OF  THE  FLOODED  VEHICLE 


PARAMETER (  WEIGHT  -12000.,  BOY 

-12000.  , VOL  -200. 

, XG  -  0. 

£  YG  -  0.0 

,  ZG  -  0.20 

, XB  -  0. 

,  ZB  -  0.0 

£  IX  -  1500. 

,  IY  -  10000. 

,IZ  -  10000. 

,IXZ  -  -10. 

£  IYZ  -  -10. 

,  IX Y  -  -10. 

, YB  -  0.0  , 

£  L  -  17.4 

,  RHO  -  1.94 

, G  -  32.2 

, NU  -  8.47E 

£  A0  -  2.0 

, KPROP  -  0. 

, NPROP  -  0.  , 

X1TEST-  0. 

£  DEGRUD-  0.0 

, DEGSTN-  0.0) 

INPUT  INITIAL 

CONDITIONS  HERE 

IF  REQUIRED 

OPEN (20 .FILE- 'DAWAY.DAT' , STATUS- ' NEW ' ) 

OPEN ( 18 , FILE- ' MODEL . DAT ' , STATUS- ' NEW ' ) 

NUMPTS-0 . 0 
DV-1 .0 

******************** ‘OBTAIN  INITIAL  INFORMATION*  *  *************** 

WRITE (  *  ,  *  )  'PROVIDE  INITIAL  CONDITIONS  FOR  SPEED  AND  RPM? ' 
READ( *  ,  *  )  U0,RPM 

WRITE ( * , * )  'FIRST  WAY  POINT  IN  SHIP  LENGTHS  (XD,YD,ZD)?' 

READ( *  ,  *  )  XD , YD , COMZ 

WRITE  (*,*)  'WHAT  IS  THE  DESIRED  SPEED?' 

READ( * , * )  UD 

WRITE (  *  ,  *  )  'HOW  LONG  BETWEEN  UPDATES  FROM  THE  NAVIGATOR?' 

READ  (*,*)  NAVUPDATE 

WRITE( * i * )  '  INPUT  TARGET  RADIUS  IN  SHIP  LENGTHS.' 

READ  ( *  ,  *  )  TARGET 

WRITE (  *  ,  *  )  'INPUT  DRIFT  VELOCITIES  UCO,VCO,WCO  IN  FT/SEC.' 

READ  (*,*)  UCO,VCO,WCO 

WRITE( *  ,  *  )  'INPUT  DRIFT  VELOCITIES  EST . UCE , VCE , WCE  IN  FT/SEC.' 

READ  (*,*)  UCE, VCE, WCE 

XD1-0.0 

YDl-0.0 

XD2-XD 

YD2-YD 

DY-  (YD2-YD1) 

DX-  ( XD2-XD1 ) 

ALPHA-ATAN2 ( DY , DX ) 

VO  -  0.0 
WO  -  0.0 
P0  -  0.0 
QO  -  0.0 
R0  -  0.0 
PHI0  -  0.0 
THETA0  -  0.0 
PSI0  -  0.0 
XPOSO-O.O 
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YPOSO-O.O 
ZPOSO-0 . 0 
DB-  0.0 
DS  -  0.0 
DR  -  0.0 
LATYAW  -  0.0 
NORPIT  -  0.0 
RE  -  U0*L/NU 
TNAV-0 
XA-XPOSO 
YA-YPOSO 
LDAWAY  -  2.0 
NEARHISS  -  0.0 

U  -  U0 
V  -  V0 
W  -  W0 
P  -  P0 
Q  -  Q0 
R  -  R0 

XPOS  -  XPOSO 
YPOS  -  YPOSO 
ZPOS  -  ZPOSO 
PSI  -  PHI0 
THETA  -  THETA0 
PHI  -  PHI0 
QHADOT-O.O 
THADOT-O.O 
ZHADOT-0 . 0 
QHAT-0.0 
THAT-0 . 0 
ZHAT=0 . 0 
VHAT-0.0 
ZOBSDOT-0.0 
ZZOBS  -  0.0 

DEFINE  LENGTH  FRACTIONS  FOR  GAUSS  QUADRATURE  TERMS 

G4(l)  -  0.069431844 
G4 (  2  )  -  0.330009478 
G4 (  3  )  -  0.669990521 
G4(4)  -  0.930568155 

DEFINE  WEIGHT  FRACTIONS  FOR  GAUSS  QUADRATURE  TERMS 

GK4 (  1  )  -  0.1739274225687 
GK4 (  2  )  -  0.3260725774312 
GK4 ( 3 )  -  0.3260725774312 
GK4 (  4  )  -  0.1739274225687 

DEFINE  THE  BREADTH  BB  AND  HEIGHT  HH  TERMS  FOR  THE  INTEGRATION 


BR  ( 1 ) 

- 

IS. 1/12 

BR(  2  ) 

- 

75.7/12 

BR  (  3  ) 

- 

75. 7/12 

BR  (  4  ) 

- 

55.08/12 

HH  (  1  ) 

m 

16.38/12 

HH  (  2  ) 

m 

31.85/12 

HH  (  3  ) 

- 

31.85/12 
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HH ( 4 )  -  23.76/12 
C 

MASS  -  WEIGHT/G 
C 

N  -  6 

DO  15  J  -  1 ,  N 
DO  10  K  -  1 , N 
XMMINV(J,K)  «=  0.0 
MM(J,K)  -  0.0 

10  CONTINUE 
15  CONTINUE 

C 

MM (1,1)  -  MASS  -( (RHO/2)*(L**3)*XUDOT) 

MM(1,5)  -  MASS  *  ZG 
MM (1,6)  -  -MASS* YG 
C 

MM(2,2)  -  MASS  -*■  (  (  RHO/2  )  *  (  L*  *  3  )  *  YVDOT  ) 

MM( 2,4)  -  -MASS  *  ZG  - ( ( RHO/2 ) * ( L* *  4 ) * YPDOT ) 
MM(2,6)  -  MASS*XG  -  ( ( RHO/2 ) * ( L* *  4 ) * YRDOT ) 

C 

MM(3,3)  -  MASS  -  ( (RHO/2)*(L**3)*ZWDOT) 

MM (3,4)  -  MASS*YG 

MM( 3,5)  -  -MASS*XG  - ( ( RHO/2 ) * ( L* * 4 ) * ZQDOT ) 

C 

MM( 4 , 2 )  -  -MASS*ZG  -  ( (RHO/2)*(L**4 )*KVDOT) 
MM(4,3)  -  MASS* YG 

MM( 4,4)  -  IX  -  ( (RHO/2)*(L**5)*KPDOT) 

MM (4,5)  -  -IXY 

MM (4,6)  -  -IXZ  -( (RHO/2)*(L**5)*KRDOT) 

C 

MM (5,1)  -  MASS*ZG 

MM( 5 , 3 )  -  -MASS *XG  -( (RHO/2)*(L**4)*MWDOT) 

MM( 5 , 4 )  -  -IXY 

MM (5,5)  -  IY  -( (RHO/2 ) * ( L**5 ) *MQDOT) 

MM (5,6)  -  -IYZ 
C 

MM (6,1)  -  -MASS*YG 

MM (6,2)  -  MASS *XG  - ( ( RHO/2 ) * ( L* *  4 ) *NVDOT ) 

MM (6,4)  -  -IXZ  -  ( (RHO/2)*(L**5)*NPDOT) 

MM( 6,5)  -  -IYZ 

MM( 6,6)  -  IZ  -  ( (RHO/2)*(L**5)*NRDOT) 
LAST-N*N+3*N 
DO  20  M- 1 , LAST 
WKAREA(M)  -  0.0 
20  CONTINUE 
C 

IER  -  0 
IA  -  6 
IDGT  -  4 

C  *  *  *  * ‘ROUTINE  FOR  INVERTING  THE  MM  MATRIX***** 

DO  12  I-1,N 
DO  11  J«1,N 

XMM I NV ( I , J ) - 0 . 0 

11  CONTINUE 
XMMINV( I , I )-l 

12  CONTINUE 

CALL  I NVTA { MM , N , I NDX , D ) 

DO  13  J-1,N 

CALL  INVTB ( MM , N , I NDX , XMMINV( 1 , J ) ) 

13  CONTINUE 
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************ 


INPUTS 


************* 


RUDDER  AND  DIVE  PLANE  COMMANDS 

WRITE( * , * )  'INPUT  SIMULATION  TIME  AND  TIME  STEP' 

READ  {*,*)  SIM , DELT 

TIME-0.0 

DS-  0.0 

DR-  0.0 

DB-  0.0 

EITA-4 . 0 

BAR- . 4 

SIZE  OF  OUTPUT  DATA  ARRAY  FOR  PLOTTING 
NUMOUT— 6 

11  FORMAT  (214) 

*******************SX MULATI ON  BEGINS  **************** 

DO  100  1-1 , SIM 

PROPULSION  MODEL 

SIGNU  -  1.0 

IF  (U.LT.0.0)  SIGNU  -  -1.0 
IF  (ABS(U) .LT.XlTEST)  U  -  XlTEST 
SIGNN  -  1.0 

IF  (RPM.LT.0.0)  SIGNN  -  -1.0 
ETA  -  0 . 012*RPM/U 
RE  -  U*L/NU 

CD0  -  .00385  +  ( 1 . 296E- 1 7 ) * ( RE  -  1.2E7)**2 

CT  -  ABS(0.008*L**2*ETA*ABS(ETA)/(A0) ) 

CTl  -ABS (  0 . 008*L* * 2/( A0 ) ) 

EPS  -  -1 . 0+SIGNN/S IGNU* ( SQRT( CT+ 1 . 0 ) -1 . 0 )/( SQRT( CTl+1 . 0 ) -1 . 0 ) 
XPROP  -  CD0* ( ETA*ABS( ETA)  -  1.0) 


CALCULATE  THE  DRAG  FORCE,  INTEGRATE  THE  DRAG  OVER  THE  VEHICLE 
INTEGRATE  USING  A  4  TERM  GAUSS  QUADUTURE 


00 


LATYAW  -  0.0 
NORPIT  -  0.0 
DO  500  K  -  1,4 

UCF(K)  -  SQRT( (V+G4(K)*R*L)**2  +  ( W-G4 ( K ) *Q*L ) * *2 ) 
I F ( UCF ( K ) . GT . IE-10 )  THEN 

TERM0  -  (RHO/2)*(CDY*HH(K)*(V+G4(K)*R*L)**2  + 
CDZ*BR(K)*(W-G4(K)*Q*L)**2) 

TERM1  -  TERM0*(V+G4(K)*R*L)/UCF(K) 

TERM2  -  TERM0*(W-G4(K)*Q*L)/UCF(K) 
l  vTYAW  -  LATYAW  +  TERMl *GK4 ( K  )  *L 
NORPIT  -  NORPIT  +  TERM2 *GK 4 ( K  )  * L 
END  IF 
CONTINUE 


FORCE  EQUATIONS 


LONGITUDINAL  FORCE 


FP ( 1 )  -  MASS*V*R  -  MASS*W*Q  +  MASS*XG*Q**2  +  MASS *XG*R* * 2- 
l  MASS* YG*P*Q  -  MASS*ZG*P*R  +  ( RHO/2 ) *L* * 4  * ( XPP*P* *2  + 
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&  XQQ*Q*  *  2  +  XRR*R**2  +  XPR*P*R)  +  ( RHO/2 ) *L* *  3* ( XWQ*W*Q  + 

&  XVP  * V*P  +  XVR*V*R  +  U*Q* ( XQDS  *  DS  +  XQDB*  DB ) +XRDR*U*R*DR )  + 

&  ( RHO/2 ) *L*  *2  * ( XVV*V*  *  2  +  XWW*W**2  +  XVDR*U*V*DR  +  U*W* 

&  (XWDS*DS+XWDB*DB)+U**2* ( XDSDS*DS**2+XDBDB*DB**2+ 

&  XDRDR*DR*  *  2 ) ) - ( WEIGHT  -BOY ) *  SIN ( THETA )  + ( RHO/2 ) * L* *  3 * 

&  XQDSN*U*Q*DS*EPS+( RHO/2 ) * L *  *  2 * ( XWDSN*U*W*DS+XDSDSN*U* *2* 

&  DS*  *2 ) *EPS  + ( RHO/2 )*L**2*U**2  *  X PROP 

LATERAL  FORCE 

FP ( 2 )  -  -MASS*U*R  -  MASS  *XG * P *0  +  MASS*YG*R**2  -  MASS*ZG*Q*R  + 

&  ( RHO/2 )*L**4*(YPQ*P*Q  +  YQR*Q*R ) + ( RHO/2 )*L**3*(YP*U*P  + 

&  YR*U*R  +  YVQ*V*Q  +  YWP  * W*  P  +  YWR*W*R)  +  ( RHO/2 ) *L* *  2 * 

&  (YV*U*V  +  YVW*V*W  +YDR*U**2*DR)  -LATYAW  + (WEIGHT-BOY ) * 

&  COS ( THETA ) *S IN( PHI )+MASS*W*P+MASS*YG*P**2 

NORMAL  FORCE 

FP(3)  -  MASS*U*Q  -  MASS  * V*  P  -  MASS*XG*P*R  -  MASS* YG*Q*R  + 

&  MASS*ZG*P**2  +  MASS*ZG*Q**2  +  ( RHO/2 ) *L* *  4  * ( ZPP*P* *  2  + 

5  ZPR*P*R  +  ZRR*R*  *  2 )  +  ( RHO/2 ) *L**3* ( ZQ*U*Q  +  ZVP*V*P  + 

6  ZVR*V*R )  + ( RHO/2 )*L**2*(ZW*U*W  +  ZVV*V**2  +  U**2*(ZDS* 

&  DS+ZDB*DB ) ) -N0RPIT+ (WEIGHT-BOY ) *C0S ( THETA ) *COS( PHI ) + 

&  ( RHO/2 ) *L*  *  3  *ZQN*U*Q*EPS  +( RHO/2 ) *L**2* ( ZWN*U*W  +ZDSN* 

6  U**2*DS)*EPS 

ROLL  FORCE 

FP ( 4 )  -  -IZ*Q*R  + 1 Y*Q* R  -IXY*P*R  +IYZ*Q**2  -IYZ*R**2  +IXZ*P*Q  + 
&  MASS*YG*U*Q  -MASS * YG* V* P  -MASS *ZG*W* P+ ( RHO/2 )* L* * 5* ( KPQ* 

&  P*Q  +  KQR*Q*R )  + ( RHO/2 )*L**4*(KP*U*P  +KR*U*R  +  KVQ*V*Q  + 

&  KWP*W*P  +  KWR*W*  R )  +(RHO/2)*L**3*(KV*U*V  +  KVW*V*W)  + 

&  ( YG*WEIGHT  -  YB*BOY) *  COS ( THETA )*COS(PHI)  -  ( ZG*WEIGHT  - 

&  ZB  *  BOY ) *COS (THETA)*SIN(PHI )  +  ( RHO/2 ) * L* *  4  * KPN*U* P*EPS  + 

&  ( RHO/2 )*L**3*U**2  *KPROP  +MASS*2G*U*R 

PITCH  FORCE 

FP ( 5 )  -  -IX*P*R  +IZ*P*R  +IXY*Q*R  -IYZ*P*Q  -IXZ*P**2  +IXZ*R**2  - 
6  MASS*XG*U*Q  +  MASS*XG*V*P  +  MASS*ZG*V*R  -  MASS*ZG*W*Q  + 

&  ( RHO/2 ) *L*  *  5* ( MPP*P*  *  2  +MPR*P*R  +MRR*R* *2 )  +  ( RHO/2 ) *L* *  4  * 

&  (MQ*U*Q  +  MVP*V*P  +  MVR*V*R )  +  ( RHO/2 ) *L**3* ( MW*U*W  + 

&  MVV*V*  *2+U*  *2  * ( MDS*DS  +  MDB*DB ) ) +  NORPIT  - ( XG*WEIGHT- 

&  XB*BOY ) *COS ( THETA ) *COS (PHI )  +  ( RHO/2 ) *L*  *  4  *MQN*U*Q*EPS  + 

Sr  (  RHO/2  )  *L*  *  3  *  (  MWN*U*W+MDSN*U*  *  2*DS  ) *EPS- 

6  ( ZG*WEIGHT-ZB*BOY ) *SIN( THETA ) 

YAW  FORCE 

FP ( 6 )  -  -I Y*P*Q  +IX*P*Q  +IXY*P**2  -IXY*Q**2  +IYZ*P*R  -IXZ*Q*R  - 
&  MASS*XG*U*R  +  MASS*XG*W*P  -  MASS*YG*V*R  +  MASS*YG*W*Q  + 

&  (RHO/2)*L**5*(NPQ*P*Q  +  NQR*Q*R )  + ( RHO/2 ) *L* *  4  * ( NP*U*P+ 

&  NR*U*R  +  NVQ*V*Q  +NWP*W*P  +  NWR*W*R)  +  ( RHO/2 ) * L* *  3 * ( NV* 

5  U*V  +  NVW*V*W  +  NDR*U**2*DR)  -  LATYAW  +  ( XG*WEIGHT  - 

6  XB  *  BOY ) *COS ( THETA )*SIN(PHI)  +  <  YG* WEIGHT ) *S IN( THETA ) 

&  +( RHO/2 ) *L**3*U**2*NPROP-YB*BOY*SIN( THETA) 


NOW  COMPUTE  THE  F(l-6)  FUNCTIONS 
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DO  600  J  -  1,6 

F(J)  -  0.0 
DO  600  K  -  1,6 

F(  J )  -  XMMINV ( J , K ) *  FP ( K )  +  F(J) 

600  CONTINUE 

THE  LAST  SIX  EQUATIONS  COME  FROM  THE  KINEMATIC  RELATIONS 

FIRST  SET  THE  DRIFT  CURRENT  VALUES 

UCO  -  0.0 

VCO  -  0.0 

WCO  -  0.0 

INERTIAL  POSITION  RATES  F(7-9) 

F( 7 )  -  UCO  +  U*COS(PSI)*COS{ THETA )  +  V* ( COS ( PSI ) *SIN( THETA ) * 

5  SIN(PHI)  -  S I N ( PS  I ) *COS(PHI ) )  +  W* ( COS ( PS I ) * SIN ( THETA ) * 

6  COS ( PHI )  +  SIN( PSI ) *SIN( PHI ) ) 

F ( 8 )  -  VCO  +  U*SIN{ PSI ) *COS  ( THETA )  +  V* ( S IN ( PS I ) * S I N ( THETA ) * 

6  SIN(PHI)  +  COS( PSI ) *COS{ PHI ) )  +  W* ( S IN ( PSI )* SIN ( THETA ) * 

6  COS(PHI)  -  COS ( PSI ) *SIN( PHI ) ) 

F { 9 )  -  WCO  -  U*SIN( THETA)  +V*COS ( THETA ) *S I N( PHI )  +W*COS ( THETA ) * 
&  COS (PHI) 

EULER  ANGLE  RATES  F( 10-12) 

F ( 1 0 )  -  P  +  Q* SIN ( PHI ) *TAN( THETA)  +  R*COS ( PHI ) *TAN( THETA ) 

F (  1 1 )  -  Q*COS ( PHI )  -  R* S I N ( PHI ) 

F( 12 )  -  Q*SIN(PHI) /COS ( THETA )  +  R*COS ( PHI )/COS { THETA) 

UDOT  -  F ( 1 ) 

VDOT  -  F ( 2 ) 

WDOT  -  F( 3) 

PDOT  -  F ( 4  ) 

QDOT  -  F(5) 

RDOT  -  F(6) 

XDOT  -  F(7) 

YDOT  -  F  (  8 ) 

2 DOT  -  F ( 9  ) 

PHIDOT  -  F ( 1 0  ) 

THETAD  -  F ( 1 1  ) 

PS I  DOT  -  F( 12  ) 

*******  CREATE  OUTPUT  DATA  FILE  ************************ 

IF  (I  .EQ.  DV  )  THEN 
TIMER- FLOAT ( I )/2 . 

WRITE  (20,*)  I 
WRITE  (20,744)  DAWAY 
WRITE  (18,*)  I 

WRITE  (18,743)  DS/. 01745  , DR/. 01745 

WRITE  (18,744)  XPOS/L,YPOS/L,ZPOS/L,XD, YD, COMZ, DAWAY 
WRITE  (18,745)  U , UD , RPM 

743  FORMAT  (2E11.3) 

744  FORMAT  (6E12.4) 
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745 


FORMAT  (6E12.4) 
NUMPTS-NUMPTS  +  1 
DV-DV+1 . 0/DELT 
ENDIF 

FIRST  ORDER  INTEGRATION 


U  -  U  +  DELT*UDOT 
V  -  V  +  DELT*VDOT 
W  -  W  +  DELT*WDOT 
P  -  P  +  DELT*PDOT 
Q  -  Q  +  DELT*QDOT 
R  -  R  +  DELT*RDOT 


U  -  SURGE  RATE 
V  -  SWAY  RATE 
W  -  HEAVE  RATE 
P  -  ROLL  RATE 
Q  -  PITCH  RATE 
R  -  YAW  RATE 


XPOS  - 

XPOS 

+ 

DELT*XDOT 

X  - 

SURGE 

YPOS  - 

YPOS 

+ 

DELT* YDOT 

Y  - 

SWAY 

ZPOS  - 

ZPOS 

DELT*ZDOT 

Z  - 

HEAVE 

PHI  - 

PHI  + 

DELT*PHIDOT 

PHI 

-  ROLL 

THETA 

-  THETA 

+  DELT*THETAD 

THETA  -  PI' 

PSI  - 

PSI  + 

DELT*PSIDOT 

PSI 

-  YAW 

**************************************************************** 
*  * 

*  CONTROL  LAWS  * 

*  * 


***************  SLIDING  MODE  DEPTH  CONTROL  ******************* 
CALL  OBSER ( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT , DELT , ZPOS , DS , UO ) 

S-QHAT  +  0 . 52*THAT  -  0 . 0112* ( ZHAT-COMZ*L ) 

I F ( ABS ( S )  .LT.  BAR)  SAT- ( S/BAR ) 

I F ( S  .LE.  -BAR)  SAT--1.0 

I F ( S  .GE.  BAR)  SAT-1.0 

UHAT--5 . 1429*QHAT  +  1.0714*THAT 

UBAR-EITA*SAT 

DE-UHAT+UBAR 

IF  (DE  .GE.  0.4)  DS-0.4 

IF  (DE  .LE.  -0.4)  DS--0.4 

I F (  (DE  .LT.  0.4)  .AND.  (DE  . GT .  -0.4))  DS-DE 
DB--DS* 1 . 0 

*****************  SLIDING  MODE  STEERING  CONTROL  *************** 

*************************  PLANNER  *************************** 

DAWAY- ( ( XPOS/L-XD ) *  *  2+ ( YPOS/L-YD ) *  *  2 ) *  * . 5 
IF  (  DAWAY  .LE.  TARGET  )  THEN 

WRITE( *  ,  *  )  'CURRENT  POSITION  IS  ' , XPOS/L , YPOS/L , ZPOS/L 
WRITE( * , * )  'SIMULATION  TIME  IS  ',1 
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WRITE (  *  ,  *  )  'WHAT  IS  THE  NEXT  WAY  POINT  (XD,YD,ZD)?' 

READ  (*,*)  XD, YD, COM2 

XD1-XD2 

YD1-YD2 

XD2-XD 

YD2-YD 

DY-  (YD2-YD1) 

DX-  (XD2-XD1) 

ALPHA-ATAN2 ( DY , DX ) 

WRITE  (*,*)  'WHAT  IS  THE  DESIRED  SPEED?' 

READ  (  *  ,  *  )  UD 
ENDIF 

*************************  navigator  ********************** 

IF  ( ( TIME-TNAV )  .GE.  NAVUPDATE  )  THEN 

XA-XPOS/L 

YA-YPOS/L 

TNAV-TNAV+NAVUPDATE 

ENDIF 

**************************  HEADING  *********************** 

I F ( ABS(XD-XA) .  LE.  0 . 0 00000 0 1 ) THEN 

I F ( ( YD-YA) .GT.0.0)  HD  -  2.*ATAN(1.) 

I F ( (YD-YA) .LT.0.0)  HD  -  -  2.*ATAN(1.) 

ELSEIF  ( (XD-XA) .LT.0.0  .AND.  (YD-YA)  .GT.  0.0)  THEN 
HD  -  ATAN ( ( YD-YA ) /( XD-XA ) )  +  4.*ATAN(1.) 

ELSEIF  ( (XD-XA) .LT.0.0  .AND.  (YD-YA)  . LT .  0.0)  THEN 
HD  -  ATAN( (YD-YA)/( XD-XA) )  -  4.*ATAN(1.) 

ELSE 

HD  -  ATAN( (YD-YA )/( XD-XA ) ) 

ENDIF 

HDMDEG  -  HDM*  360 . /(  8 . *ATAN ( 1 . ) ) 

**********************  SWAY  VELOCITY  OBSERVER  ***************** 

VHAT  -  0.0 

THE  VALUE  OF  THE  COEFFICIENTS  VARY  WITH  SPEED  TO  ACCOUNT  FOR  DIFFERENCES 
BETWEEN  THIS  NONLINEAR  MODEL  AND  THE  LINEARIZED  MODEL  THE  OBSERVER  IS 
BASED  ON. (THIS  CODE  WAS  NOT  USED,  INSTEAD  VHAT  WAS  SET  EQUAL  TO  ZERO.) 

FF  -  -2.0 

LLL-  -2.275*(U**6)+48.102*(U**5)-40t.839*(U**4)+ 
t  1763. 181*(U**3)-4165.344*(U**2)+5265. 448* U- 3211. 956 
GG  -  1 . 1329*U  -  70.535 
HHH  -  . 07584 * ( U* * 2  )  -  3 . 0  509 *U  +  .0003 
ZZOBSDOT  -  HHH*DR  +  GG*R  +  FF*VHAT 
ZZOBS  -  ZZOBS  +  ZZOBSDOT*DELT 
VHAT  -  ZZOBS  +  LLL*R 

****************  CONDITIONS  TO  ALLOW  SHORTEST  TURN  ************ 

HDP  -  HD  +  8 . *ATAN ( 1 . ) 

IF  ( ABS (HDP-PSI )  .LT.  ABS(HD-PSI))  THEN 
HDM  -  8  *ATAN ( 1 .  )  +  HD 
ELSE 
HDM-HD 
ENDIF 
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*****************  BEGIN  HEADING  SMC  CALCULATIONS  ************** 
VCC-VCE*COS( ALPHA)  -UCE* S IN ( ALPHA ) 

SS2-.4767*( PSI -HDM )  +  ,0121*VHAT  +  .8790*R  +  . 4767*ASIN(VCC/U) 

I  F  ( ABS ( SS2 )  .LT.  .1)  SATSGN2-( SS2/. 10 ) 

I F (  SS2  .LE.  -.1)  SATSGN2  — 1.0 
I F ( SS2  .GE.  .1)  SATSGN2-1.0 

DR-  3 . 1007*SATSGN2  +  .  137  3 * VHAT  +  .  5081*R 
IF  (DR  .GE.  0.4)  DR  -  0.4 

IF  (DR  .LE.  -  0.4)  DR  -  -0.4 

*******************  RpM  input  CALCULATION  ***************** 
SS1-U-UD 

I F ( ABS ( SSI )  .LT.  1.0)  SATSGNl- ( SSI/1 ) 

I  F  (  SSI  .LE.  -1.0)  SATSGNl  — 1.0 
I F ( SS 1  .GE.  1.0)  SATSGNl - 1 . 0 
RPM— 4000 . 0*SATSGN1  +  83.33*U 
IF  (RPM  .GE.  5000.0)  RPM-  5000.0 
IF  (RPM  .LE.  -5000.0)  RPM—5000 . 0 

TIME-TIME+DELT 


PHIANG  -  PHI/0.0174532925 
THEANG  -  THETA/0.0174532925 
PSIANG  -  PSI/0. 0174532925 


TRAC— YPOS 
ROLL-PHIANG 
YAW-PSIANG 
DEPTH— ZPOS 
P^TCH-THEANG 
BOWANG-(DB/. 01745) 
STNANG-(DS/. 01745) 
00  CONTINUE 

WRITE (  *  ,  *  ) 

WRITE ( * , * ) 

WRITE ( * , * ) 

WRITE ( * , * ) 

WRITE (  *  ,  *  ) 

WRITE ( * , * ) 

STOP 
END 


' NPTS  -  ' , NUMPTS 

'TIMEINTERVAL  -  ' , DELT 
'NAVIGATOR  UPDATE  TIME  -  ' , NAVUPDATE 
'TARGET  RADIUS  -  '.TARGET 

'NONDIM  SPEED  -  ',(UCO**2  +VCO* *  2 ) *  * . 5/4 
'NEARMISS  -  '.NEARMISS 


DEPTH  CONTROL  OBSERVER  ********************* 

SUBROUTINE  OBSER( QHADOT , THADOT, ZHADOT, QHAT, THAT, ZHAT, DELT, ZPOS , 
DS ,  U ) 

C 

QHADOT—  0 . 7  *QHAT- 0 . 0  3  *  THAT- 0 . 0  3  5  *  DS  -  2  0 . 9  2  9  3  *  (  Z  POS  -  ZHAT ) 
THADOT-QHAT-14 . 4092* ( ZPOS-ZHAT) 

ZHADOT— 6*THAT+16 . 4  5*  ( ZPOS-ZHAT) 

C 

QHAT-  QHAT+ DELT* QHADOT 
THAT-  THAT+DELT* THADOT 
ZHAT-  ZHAT+DELT* ZHADOT 
RETURN 
END 
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APPENDIX  C 


****************************************************************** 

*  THIS  PROGRAM  PLOTS  THE  OUTPUT  GRAPHS  FOR  THE  SIMULATION  OF  THE  * 

*  NONLINEAR  MODEL  OF  THE  SDV-9  SIMULATION  RUNS.  * 

****************************************************************** 

REAL* 4  T(600),DS(600),DR{600) , OHS (  600  ) 

REAL* 4  XPOS ( 600 ) , VPOS ( 600 ) , ZPOS (600) 

REAL* 4  ROLL (600) .PITCH (600) , YAW(600) 

REAL* 4  RPM( 600 ) , U( 600 ) 

REAL* 4  UG(600) ,HG(600) ,UD(600) , XD (  600 ) , YD( 600 ) , COMZ  (  600 ) 

REAL* 4  DEPTH( 600 ) ,DEPTHD(600) ,DAWAY(600) 

CHARACTER* 1  ANS 

WRITE( * , * )  'INPUT  NPTS  &  THE  TIME  INCREMENT  ( DELT ) . 

READ( *  ,  *  )  NPTS, DELT 
DO  I -1, NPTS 

OPEN (18, FILE" ' MODEL . DAT ' , STATUS- ' OLD' ) 

READ( 18 , * )  INC 
T( I )  -  DELT* INC 
READ( 18 , * )  DS ( I ) , DR ( I ) 

READ( 18,* )  XPOS ( I ) , YPOS ( I ) , ZPOS ( I ) , XD( I ) , YD( I ) , COMZ ( I ) , DAWAY ( I ) 
READ( 18 , * )  U( I ) , UD( I ) , RPM( I ) 

OHS ( I ) -0 . 0 
DEPTH ( I ) -  -ZPOS ( I ) 

DEPTHD(  I )  -  -COMZ ( I ) 

ENDDO 

CALL  MXMN ( T , NPTS , TMX , TMN ) 

CALL  MXMN (DS, NPTS, DSMX.DSMN) 

CALL  MXMN (DR, NPTS, DRMX,DRMN) 

CALL  MXMN ( XPOS , NPTS , XPOSMX , XPOSMN ) 

CALL  MXMN( YPOS , NPTS , YPOSMX , YPOSMN ) 

CALL  MXMN ( DEPTH , NPTS , DEPTHMX , DEPTHMN ) 

CALL  MXMN ( DEPTHD , NPTS , DEPTHDMX , DEPTHDMN ) 

CALL  MXMN (U, NPTS, UMX,UMN) 

CALL  MXMN (UD, NPTS, UDMX,UDMN) 

CALL  MXMN( RPM , NPTS , RPMMX, RPMMN ) 

CALL  MXMN (XD, NPTS, XDMX.XDMN) 

CALL  MXMN (YD, NPTS, YDMX,YDMN) 

CALL  MXMN ( DAWAY , NPTS , DAWAYMX , DAWAYMN ) 

C 

CALL  SCALE ( TMN , TMX ,4.1, TORIG , TSTP , TMAX ) 

C 

I F ( DSMX-DSMN  .EQ.  0.0)  THEN 

DSORIG-- . 1 

DSSTP-. 1 

DSMAX- . 1 

ELSE 

CALL  SCALE( DSMN, DSMX , 4 . 1 , DSORIG , DSSTP , DSMAX ) 

EN'JIF 

C 

I F ( DRMX-DRMN  .EQ.  0.0)  THEN 

DRORIG— .  1 

DRSTP-  .  1 

DRMAX-.l 

ELSE 
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c 


c 


c 


c 


c 


c 


c 


CALL  SCALE( DRMN , DRMX ,4.1, DRORIG , DRSTP , DRMAX ) 

ENDIF 

I F ( XPOSMX-XPOSMN  . EQ .  0.0)  THEN 

XPOSORIG— .1 

XPOSSTP-.l 

XPOSMAX-. 1 

ELSE 

CALL  SCALE { XPOSMN ,XPOSMX, 4 . 1 , XPOSORIG , XPOSSTP , XPOSMAX ) 

ENDIF 

XXMX-AMAXl ( XPOSMX , XDMX ) 

XXMN-AM1N1 ( XPOSMN, XDMN) 

1 F ( XXMX-XXMN  .EQ.  0.0)  THEN 

XXORIG--. 1 

XXSTP-.l 

XXMAX-. 1 

ELSE 

CALL  SCALE ( XXMN , XXMX ,6.1, XXORIG , XXSTP , XXMAX ) 

ENDIF 

YYMX-AMAXl ( YPOSMX , YDMX ) 

Y YMN-AMINl ( YPOSMN , YDMN ) 

I F ( YYMX-YYMN  . EQ .  0.0)  THEN 

YYORIG—  .1 

YYSTP-.l 

YYMAX-.l 

ELSE 

CALL  SCALE ( YYMN , YYMX ,6.1, YYORIG, YYSTP , YYMAX ) 

ENDIF 

DDMX-AMAXl ( DEPTHMX , DEPTHDMX ) 

DDMN-AMINl ( DEPTHMN , DEPTHDMN ) 

I F ( DDMX-DDMN  .EQ.  0.0)  THEN 

DDORIG— .  1 

DDSTP-.l 

DDMAX-.l 

ELSE 

CALL  SCALE ( DDMN , DDMX ,6.1, DDORIG, DDSTP , DDMAX ) 

ENDIF 

I F ( DAWAYMX-DAWAYMN  . EQ .  0.0)  THEN 

DAWAYORIG— .1 

DAWAYSTP- . 1 

DAWAYMAX- . 1 

ELSE 

CALL  SCALE ( DAWAYMN , DAWAYMX ,6.1, DAWAYORIG , DAWAYSTP , DAWAYMAX ) 
ENDIF 

HGMX-AMAX1 ( PS I MX , HDMX ) 

HGMN-AM I N1 ( PS I MN , HDMN ) 

I F( HGMX-HGMN  .EQ.  0.0)  THEN 

HGORIG—  .1 

HGSTP-.l 

HGMAX-.l 

ELSE 

CALL  SCALE ( HGMN , HGMX ,6.1, HGOR I G , HGSTP , HGMAX ) 

ENDIF 

UGMX-AMAX1 ( UMX , UDMX ) 
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UGMN-AHINl ( UMN , UDMN ) 

I F ( UGMX-UGMN  . EQ .  0.0)  THEN 

UGORIG--.1 

UGSTP-. 1 

UGMAX- . 1 

ELSE 

CALL  SCALE ( UGMN , UGMX , 6 . 1 , UGOR I G , UGSTP , UGMAX ) 

ENDIF 

I F ( RPMMX-RPHMN  . EQ .  0.0)  THEN 

RPMORIG—  .  1 

RPMSTP-  .  1 

RPMMAX-.l 

ELSE 

CALL  SCALE ( RPMMN , RPMMX ,4.1, RPM6RI G , RPMSTP , RPMMAX ) 

ENDIF 

**********************  PLOT  OR  VIEW- OPTION  ************************** 

WRITE( * , * )  'DO  YOU  WANT  TO  VIEW  THE  PLOT  OR  OBTAIN  A  HARDCOPY? ' 
WRITE(  *  ,  * )  '  ' 

WRITE ( * , * )  'INPUT  1  FOR  VIEW  OR  2  FOR  HARDCOPY* 

READ(  *  ,  * )  IPLOTVAL 
I F ( I PLOT_VAL  .EQ.  1)  CALL  PGPX 
IFflPLOTVAL  .EQ.  2)  CALL  LN03I 

*************  HORIZONTAL  -POSITION  ********************* 

CALL  PAGE (8.5, 11.0) 

CALL  NOBRDR 

CALL  PHYSOR( 1.75,7.75) 

CALL  AREA2D( 2 . 0 , 2 . 0 ) 

CALL  XTICKS ( 1 ) 

CALL  YTICKS ( 1 ) 

CALL  XNAME ( ' X  (SHIP  LENGTHS )$', 1 00  ) 

CALL  YNAME ( ' Y  (SHIP  LENGTHS )$', 1 00 ) 

CALL  GRAF ( XXORIG , XXSTP , XXMAX , YYORIG , YYSTP , YYMAX ) 

IMARK-0 

CALL  RESET( 'DOT' ) 

CALL  RESET( 'THKCRV' ) 

CALL  GRID( 1,1) 

CALL  THKCRV( 4 ) 

CALL  CURVE ( XPOS , YPOS , NPTS , I MARK ) 

CALL  MARKER (13) 

CALL  SCLPIC( 2 ) 

IMARK  — 1 

CALL  CURVE (  XD ,  YD ,  NPTS  ,  IMAR.K  ) 

CALL  ENDGR(O) 

***************  STERN  PLANE  INPUTS  ******************** 

CALL  PHYSOR( 4.95,5.05) 

CALL  AREA2D( 2 . 0 , 2 . 0  ) 

CALL  XTICKS ( 1 ) 

CALL  YTICKS ( 1 ) 

CALL  XNAME( ' TIME  (SEC)$',100) 

CALL  YNAME( 'STERN  PL.  ( DEGREES )§', 1 00 ) 

CALL  GRAF ( TORIG , TSTP , TMAX , DSOR IG , DSSTP , DSMAX ) 

IMARK-0 

CALL  RESET('DOT') 
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CALL  RESET ( 'THKCRV' ) 

CALL  GRID( 1,1) 

CALL  THKCRV ( 4 ) 

CALL  CURVE ( T, DS , NPTS , I MARK ) 

CALL  ENDGR(O) 

********************  RUDDER  INPUTS  ******************* 

CALL  PHYSORJ  4.95,7.75) 

CALL  AREA2D<2. ,2. ) 

CALL  XTICKS(l) 

CALL  YTICKS(l) 

CALL  Y1NTAX 

CALL  XNAME( 'TIME  (SEC)$',100) 

CALL  YNAME< 'RUDDER  ( DEGREES )$', 1 0 0 ) 

CALL  GRAF ( TORIG , TSTP , TMAX , DROR1G , DRSTP , DR MAX ) 

CALL  RESET( 'DOT' ) 

CALL  RESET ( 'THKCRV' ) 

CALL  GRID( 1,1) 

I MARK-0 

CALL  THKCRV ( 4 ) 

CALL  CURVE ( T, DR , NPTS , I MARK ) 

CALL  ENDGR(O) 

******  DISTANCE  AWAY  FROM  NEXT  WAY  POINT  (DAWAY)  ****** 

CALL  PHYSOR( 1.75,5.05) 

CALL  AREA2D( 2 . ,2. ) 

CALL  XTICKS ( 1 ) 

CALL  YTICKS ( 1 ) 

CALL  XNAME( ' TIME  (SEC)$',100) 

CALL  YNAME( 'DAWAY  (SHIP  LENGTHS)  $',100) 

CALL  GRAF ( TORIG , TSTP , TMAX , DAWAYOR IG , DAWAYSTP , DAWAYMAX ) 
CALL  RESET( 'DOT' ) 

CALL  RESET( 'THKCRV' ) 

CALL  GRID( 1,1) 

IMARK-0 

CALL  THKCRV ( 4 ) 

CALL  CURVE (T, DAWAY, NPTS, I MARK) 

CALL  ENDGR(O) 

***************  SPEED  VS  DESIRED  SPEED  *************** 

CALL  PHYSOR( 4.95,5.05) 

CALL  AREA2D(2. ,2. ) 

CALL  XTICKS ( 1 ) 

CALL  YTICKS(l) 

CALL  XNAME( ' TIME  (SEC)$',100) 

CALL  YNAME('U  AND  UD  (FT/SEC)  $*,100) 

CALL  GRAF ( TOR I G , TSTP , TMAX , UGORIG , UGSTP , UGMAX ) 

CALL  RESET ( 'DOT' ) 

CALL  RESET ( 'THKCRV' ) 

CALL  GRID( 1,1) 

IMARK-0 

CALL  THKCRV ( 4 ) 

CALL  CURVE (T,U, NPTS, I MARK) 

CALL  DOT 

CALL  CURVE( T, UD, NPTS , I MARK ) 

CALL  ENDGR(O) 
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c 
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c 

c 

c 

c 

c 

c 

c 
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100 

c 
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*********************  motor  input  ********************* 

CALL  PHYSOR( 4.95,2.35) 

CALL  AREA2D(2. ,  2.  ) 

CALL  XTICKS ( 1 ) 

CALL  YTICKS ( 1 ) 

CALL  XNAME( ' TIME  (SEC)$',100) 

CALL  YNAME( ' RPM  S', 100) 

CALL  YINTAX 

CALL  GRAF ( TORIG , TSTP , TMAX , RPMOR I G , RPMSTP , RPMMAX ) 

CALL  RESET( 'DOT' ) 

CALL  RESET( 'THKCRV' ) 

CALL  GRID( 1,1) 

IMARK-0 

CALL  THKCRV ( 2 ) 

CALL  CURVE (T, RPM, NPTS, I MARK) 

CALL  ENDPL(O) 

GOTO  100 

CONTINUE 
CALL  METAFL ( 1 ) 

CALL  ENDPL(O) 

CALL  DONEPL 

WRITE  (*,*)  '  DO  YOU  WANT  TO  CONTINUE  ?  (Y/N)' 

READ( * , 101 )  ANS 

IF  (ANS  .EQ.  ' Y' )  GOTO  4 

STOP 

FORMAT ( A) 

END 


> 
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APPENDIX  D. 


The  values  used  to  design  a  reduced  order  observer  for  sway 


velocity(v)  varied  so  much  with  respect  to  changes  in  surge  velocity(u)  that  the  following 


attempt  was  made  to  relate  them  using  curve  fitting  techniques.  The  following  is  the 


computer  code  which  resulted. 


**********************  SWAY  VELOCITY  OBSERVER  ***************** 


VHAT  -  0.0 

THE  VALUE  OF  THE  COEFFICIENTS  VARY  WITH  SPEED  TO  ACCOUNT  FOR  DIFFERENCES 
BETWEEN  THIS  NONLINEAR  MODEL  AND  THE  LINEARIZED  MODEL  THE  OBSERVER  IS 
BASED  ON. (THIS  CODE  WAS  NOT  USED,  INSTEAD  VHAT  WAS  SET  EQUAL  TO  ZERO.) 

FF  -  -2.0 

LLL-  -2.275*(U**6)+48.102*(U**5)-406.839*(U**4)+ 

&  1763. 181*(U**3)-4165.344*(U**2)+5265. 448* U- 3211. 956 

GG  -  1 . 1329*U  -  70.535 
HHH  -  . 07584* (U**2 )  -  3.0509*U  +  .0003 
ZZOBSDOT  -  HHH*DR  +  GG*R  +  FF*VHAT 
ZZOBS  -  ZZOBS  +  ZZOBSDOT*DELT 
VHAT  -  ZZOBS  +  LLL*R 


FIGURE  48.  Adaptive  Sway  Velocity  Observer  Code 

The  coefficients  in  the  above  equations  and  the  quality  of  the  curve  fitting  technique 
that  provided  them  are  shown  in  the  following  graphs.  Notice  the  wide  range  of  values 
that  occur  when  dealing  with  the  full  nonlinear  model.  This  again  shows  the  extent  to 
which  the  sliding  mode  control  laws  must  deal  with  unmodeled  behavior. 

Recall  that  ultimately  sway  velocity  was  set  equal  to  zero  and  t tented  ns  n 
disturbance. 
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